import rospy import tf from geometry_msgs.msg import Pose from std_msgs.msg import Float64 from std_msgs.msg import Int32 from std_msgs.msg import String from sensor_msgs.msg import PointCloud2 from pr2_robot.srv import * from rospy_message_converter import message_converter import yaml from sklearn.preprocessing import LabelEncoder, StandardScaler from sklearn import svm import random angle1 = Float64() angle2 = Float64() angle1.data = 1.57 angle2.data = -1.57 map_ready = True mapping_stage = 0 start_time = 0 outputed = False world_num = 3 # Helper function to get surface normals def get_normals(cloud): get_normals_prox = rospy.ServiceProxy('/feature_extractor/get_normals', GetNormals)
def twist_callback(twist_msg): raw_ang = twist_msg.angular.z steer_cmd = raw_ang #TODO: 1) Convert angular rate to steering wheel angle & 2) Limit the angle steer_pub.publish(Float64(steer_cmd))
#!/usr/bin/env python import rospy import numpy as np from std_msgs.msg import String, Empty, Float64, Int16, Bool from time import time rospy.init_node('depth_control', anonymous=False) # State value to be used by PID controller state = Float64() state.data = 0 # Depth data container depthpwm = Int16() depthpwm.data = 0 enabled = False # Publishers to send pwm value to motors feedback set_depthpwm = rospy.Publisher('/depth_pwm_feedback', Int16, queue_size=10) def vel_callback(effort_msg): depthpwm.data = effort_msg.data set_depthpwm.publish(depthpwm) # Send pwm OUTPUT of PID controller to robot def depth_callback(depth_msg): state.data = depth_msg.data set_state.publish(state) # Send depth as FEEDBACK to PID controller def reset_callback(reset_msg): enabled = reset_msg.data
from nav_msgs.msg import Odometry from geometry_msgs.msg import Quaternion from geometry_msgs.msg import Twist, Point, Quaternion from std_msgs.msg import Float64 initial_pose = Odometry() target_pose = Odometry() target_distance = 0 actuator_vel = 70 Ianterior = 0 rate_value = 10 Kp = 10 Ki = 0 left = 0 right = 0 result = Float64() result.data = 0 #desired_distance = 3 # big scenarios desired_distance = 2 # scenario G def get_pose(initial_pose_tmp): global initial_pose initial_pose = initial_pose_tmp def get_target(target_pose_tmp): global target_pose target_pose = target_pose_tmp
def set_camera_tilt(pub, tilt_rad): tilt_msg = Float64() tilt_msg.data = tilt_rad rospy.loginfo('Going to camera tilt: {} rad'.format(tilt_rad)) pub.publish(tilt_msg)
def __init__(self): #### GoPiGo3 power management # export pin if not os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN): gpio_export = os.open("/sys/class/gpio/export", os.O_WRONLY) os.write(gpio_export, self.POWER_PIN.encode()) os.close(gpio_export) time.sleep(0.1) # set pin direction gpio_direction = os.open( "/sys/class/gpio/gpio" + self.POWER_PIN + "/direction", os.O_WRONLY) os.write(gpio_direction, "out".encode()) os.close(gpio_direction) # activate power management self.gpio_value = os.open( "/sys/class/gpio/gpio" + self.POWER_PIN + "/value", os.O_WRONLY) os.write(self.gpio_value, "1".encode()) # GoPiGo3 and ROS setup self.g = gopigo3.GoPiGo3() print("GoPiGo3 info:") print("Manufacturer : ", self.g.get_manufacturer()) print("Board : ", self.g.get_board()) print("Serial Number : ", self.g.get_id()) print("Hardware version: ", self.g.get_version_hardware()) print("Firmware version: ", self.g.get_version_firmware()) self.reset_odometry() rospy.init_node("gopigo3") self.br = TransformBroadcaster() # subscriber rospy.Subscriber("motor/dps/left", Int16, lambda msg: self.g.set_motor_dps(self.ML, msg.data)) rospy.Subscriber("motor/dps/right", Int16, lambda msg: self.g.set_motor_dps(self.MR, msg.data)) rospy.Subscriber("motor/pwm/left", Int8, lambda msg: self.g.set_motor_power(self.ML, msg.data)) rospy.Subscriber("motor/pwm/right", Int8, lambda msg: self.g.set_motor_power(self.MR, msg.data)) rospy.Subscriber( "motor/position/left", Int16, lambda msg: self.g.set_motor_position(self.ML, msg.data)) rospy.Subscriber( "motor/position/right", Int16, lambda msg: self.g.set_motor_position(self.MR, msg.data)) rospy.Subscriber("servo/pulse_width/1", Int16, lambda msg: self.g.set_servo(self.S1, msg.data)) rospy.Subscriber("servo/pulse_width/2", Int16, lambda msg: self.g.set_servo(self.S2, msg.data)) rospy.Subscriber("servo/position/1", Float64, lambda msg: self.set_servo_angle(self.S1, msg.data)) rospy.Subscriber("servo/position/2", Float64, lambda msg: self.set_servo_angle(self.S2, msg.data)) rospy.Subscriber("cmd_vel", Twist, self.on_twist) rospy.Subscriber("led/blinker/left", UInt8, lambda msg: self.g.set_led(self.BL, msg.data)) rospy.Subscriber("led/blinker/right", UInt8, lambda msg: self.g.set_led(self.BR, msg.data)) rospy.Subscriber( "led/eye/left", ColorRGBA, lambda c: self.g.set_led( self.EL, int(c.r * 255), int(c.g * 255), int(c.b * 255))) rospy.Subscriber( "led/eye/right", ColorRGBA, lambda c: self.g.set_led( self.ER, int(c.r * 255), int(c.g * 255), int(c.b * 255))) rospy.Subscriber( "led/wifi", ColorRGBA, lambda c: self.g.set_led( self.EW, int(c.r * 255), int(c.g * 255), int(c.b * 255))) # publisher self.pub_enc_l = rospy.Publisher('motor/encoder/left', Float64, queue_size=10) self.pub_enc_r = rospy.Publisher('motor/encoder/right', Float64, queue_size=10) self.pub_battery = rospy.Publisher('battery_voltage', Float64, queue_size=10) self.pub_motor_status = rospy.Publisher('motor/status', MotorStatusLR, queue_size=10) self.pub_odometry = rospy.Publisher("odom", Odometry, queue_size=10) self.pub_joints = rospy.Publisher("joint_state", JointState, queue_size=10) # services self.srv_reset = rospy.Service('reset', Trigger, self.reset) self.srv_spi = rospy.Service( 'spi', SPI, lambda req: SPIResponse( data_in=self.g.spi_transfer_array(req.data_out))) self.srv_pwr_on = rospy.Service('power/on', Trigger, self.power_on) self.srv_pwr_off = rospy.Service('power/off', Trigger, self.power_off) # main loop rate = rospy.Rate(rospy.get_param('hz', 30)) # in Hz while not rospy.is_shutdown(): self.pub_battery.publish( Float64(data=self.g.get_voltage_battery())) # publish motor status, including encoder value (flags, power, encoder, speed) = self.g.get_motor_status(self.ML) status_left = MotorStatus(low_voltage=(flags & (1 << 0)), overloaded=(flags & (1 << 1)), power=power, encoder=encoder, speed=speed) self.pub_enc_l.publish(Float64(data=encoder)) (flags, power, encoder, speed) = self.g.get_motor_status(self.MR) status_right = MotorStatus(low_voltage=(flags & (1 << 0)), overloaded=(flags & (1 << 1)), power=power, encoder=encoder, speed=speed) self.pub_enc_r.publish(Float64(data=encoder)) self.pub_motor_status.publish( MotorStatusLR(header=Header(stamp=rospy.Time.now()), left=status_left, right=status_right)) # publish current pose (odom, transform) = self.odometry(status_left, status_right) self.pub_odometry.publish(odom) self.br.sendTransformMessage(transform) rate.sleep() self.g.reset_all() self.g.offset_motor_encoder(self.ML, self.g.get_motor_encoder(self.ML)) self.g.offset_motor_encoder(self.MR, self.g.get_motor_encoder(self.MR)) # deactivate power management os.write(self.gpio_value, "0".encode()) os.close(self.gpio_value) # unexport pin if os.path.isdir("/sys/class/gpio/gpio" + self.POWER_PIN): gpio_export = os.open("/sys/class/gpio/unexport", os.O_WRONLY) os.write(gpio_export, self.POWER_PIN.encode()) os.close(gpio_export)
# Extrayendo valores de X, Y, Z self.Xo = np.concatenate((self.Xo, XYZ_0[0, :])) self.Yo = np.concatenate((self.Yo, XYZ_0[1, :])) self.Zo = np.concatenate((self.Zo, XYZ_0[2, :])) #print ("Escaneo #: %i" %self.veces) #sonido.publish(1) if __name__ == '__main__': try: rospy.init_node("Recons_ang_fixed_no_color") pub = rospy.Publisher('/motor_controller/command', Float64, queue_size=10) reset = rospy.Publisher('/mobile_base/commands/reset_odometry', Empty, queue_size=10) reset.publish(Empty()) sonido = rospy.Publisher('/mobile_base/commands/sound', Sound, queue_size=10) pos = Float64() f = open( '/home/ros/Escritorio/HOKUYO/prueba hokuyo/Reconstruccion_din_Ang_fijo/Recons_Ang_Fijo.txt', 'w') cv = First() except rospy.ROSInterruptException: f.close
def control(self, sec_idle=1.0): ''' @brief Intended to be called periodically. Reads the last velocity command, interpretes for marvin mechanism, then publish as Float64 that is the data type dynamixel_controllers receive. @type sec_idle: float @param sec_idle: Ideling seconds before robot stops moving ''' control_interval = (rospy.Time.now() - self.last_control_time).to_sec() self.last_control_time = rospy.Time.now() if (rospy.Time.now() - self.cmd_time ).to_sec() > sec_idle: ## if new cmd_vel did not comes for 5 sec self.cmd = Twist() ### velocity control (raw commnd velocity, we need to filter this) accel_trans_limit = 100 accel_rotate_limit = 100 raw_linear_x = self.cmd.linear.x - self.last_cmd.linear.x raw_linear_y = self.cmd.linear.y - self.last_cmd.linear.y raw_rotate = self.cmd.angular.z - self.last_cmd.angular.z if control_interval > 0 and \ (abs(raw_linear_x)/control_interval > accel_trans_limit or \ abs(raw_linear_y)/control_interval > accel_trans_limit or \ abs(raw_rotate)/control_interval > accel_rotate_limit) : rospy.logwarn( "Too Large accel: cmd_vel %7.3f %7.3f %7.3f, cmd_vel_dot %7.3f %7.3f %7.3f" % (self.cmd.linear.x, self.cmd.linear.y, self.cmd.angular.z, abs(raw_linear_x) / control_interval, abs(raw_linear_y) / control_interval, abs(raw_rotate) / control_interval)) self.curr_cmd.linear.x = self.last_cmd.linear.x + max( min(raw_linear_x, accel_trans_limit * control_interval), -accel_trans_limit * control_interval) self.curr_cmd.linear.y = self.last_cmd.linear.y + max( min(raw_linear_y, accel_trans_limit * control_interval), -accel_trans_limit * control_interval) self.curr_cmd.angular.z = self.last_cmd.angular.z + max( min(raw_rotate, accel_rotate_limit * control_interval), -accel_rotate_limit * control_interval) rospy.logdebug("cmd_vel %f %f %f" % (self.curr_cmd.linear.x, self.curr_cmd.linear.y, self.curr_cmd.angular.z)) diameter = 0.178 # caster diameter offset_x = 0.185 # caster offset offset_y = 0.235 # # http://www.chiefdelphi.com/media/papers/download/2614 fr_v_x = self.curr_cmd.linear.x - self.curr_cmd.angular.z * (-offset_y) fr_v_y = self.curr_cmd.linear.y + self.curr_cmd.angular.z * (offset_x) fl_v_x = self.curr_cmd.linear.x - self.curr_cmd.angular.z * (offset_y) fl_v_y = self.curr_cmd.linear.y + self.curr_cmd.angular.z * (offset_x) br_v_x = self.curr_cmd.linear.x - self.curr_cmd.angular.z * (-offset_y) br_v_y = self.curr_cmd.linear.y + self.curr_cmd.angular.z * (-offset_x) bl_v_x = self.curr_cmd.linear.x - self.curr_cmd.angular.z * (offset_y) bl_v_y = self.curr_cmd.linear.y + self.curr_cmd.angular.z * (-offset_x) # v[m/s] = r[rad/s] * 0.1[m] ## 0.1 = diameter fr_v = hypot(fr_v_x, fr_v_y) fl_v = hypot(fl_v_x, fl_v_y) br_v = hypot(br_v_x, br_v_y) bl_v = hypot(bl_v_x, bl_v_y) fr_a = atan2(fr_v_y, fr_v_x) fl_a = atan2(fl_v_y, fl_v_x) br_a = atan2(br_v_y, br_v_x) bl_a = atan2(bl_v_y, bl_v_x) # fix for -pi/2 - pi/2 fr_v = -fr_v if fabs(fr_a) > pi / 2 else fr_v fr_a = fr_a - pi if fr_a > pi / 2 else fr_a fr_a = fr_a + pi if fr_a < -pi / 2 else fr_a fl_v = -fl_v if fabs(fl_a) > pi / 2 else fl_v fl_a = fl_a - pi if fl_a > pi / 2 else fl_a fl_a = fl_a + pi if fl_a < -pi / 2 else fl_a br_v = -br_v if fabs(br_a) > pi / 2 else br_v br_a = br_a - pi if br_a > pi / 2 else br_a br_a = br_a + pi if br_a < -pi / 2 else br_a bl_v = -bl_v if fabs(bl_a) > pi / 2 else bl_v bl_a = bl_a - pi if bl_a > pi / 2 else bl_a bl_a = bl_a + pi if bl_a < -pi / 2 else bl_a rospy.logdebug("wheel %f %f %f %f" % (fr_v, fl_v, br_v, bl_v)) rospy.logdebug("rotate %f %f %f %f" % (fr_a, fl_a, br_a, bl_a)) self.pub_fr_w.publish( Float64(-1 * fr_v / (diameter / 2.0))) # right wheel has -1 axis orientation self.pub_fl_w.publish(Float64(fl_v / (diameter / 2.0))) self.pub_br_w.publish(Float64(-1 * br_v / (diameter / 2.0))) self.pub_bl_w.publish(Float64(bl_v / (diameter / 2.0))) self.pub_fr_r.publish(Float64( -1 * fr_a)) # All steerage wheels' rotational direction needs self.pub_fl_r.publish(Float64( -1 * fl_a)) # to be negated since they are upside down. self.pub_br_r.publish(Float64(-1 * br_a)) self.pub_bl_r.publish(Float64(-1 * bl_a)) ## Odometry self.odom.header.stamp = rospy.Time.now() self.odom.header.frame_id = "odom" c_fr_v = c_fl_v = c_br_v = c_bl_v = c_fr_a = c_fl_a = c_br_a = c_bl_a = 0 for i in range(len(self.state.name)): if self.state.name[i] == 'bl_rotation_joint': c_bl_a = self.state.position[i] * -1 if self.state.name[i] == 'br_rotation_joint': c_br_a = self.state.position[i] * -1 if self.state.name[i] == 'fl_rotation_joint': c_fl_a = self.state.position[i] * -1 if self.state.name[i] == 'fr_rotation_joint': c_fr_a = self.state.position[i] * -1 if self.state.name[i] == 'bl_wheel_joint': c_bl_v = self.state.velocity[i] * (diameter / 2.0) if abs(c_bl_v) < 0.002: c_bl_v = 0 if self.state.name[i] == 'br_wheel_joint': c_br_v = self.state.velocity[i] * (diameter / 2.0) * -1 if abs(c_br_v) < 0.002: c_br_v = 0 if self.state.name[i] == 'fl_wheel_joint': c_fl_v = self.state.velocity[i] * (diameter / 2.0) if abs(c_fl_v) < 0.002: c_fl_v = 0 if self.state.name[i] == 'fr_wheel_joint': c_fr_v = self.state.velocity[i] * (diameter / 2.0) * -1 if abs(c_fr_v) < 0.002: c_fr_v = 0 offset = sqrt(offset_x * offset_x + offset_y * offset_y) curr_linear_x = (cos(c_fl_a) * c_fl_v + cos(c_fr_a) * c_fr_v + cos(c_br_a) * c_br_v + cos(c_bl_a) * c_bl_v) / 4 curr_linear_y = (sin(c_fl_a) * c_fl_v + sin(c_fr_a) * c_fr_v + sin(c_br_a) * c_br_v + sin(c_bl_a) * c_bl_v) / 4 # rospy.loginfo("%f %f %f %f" % (c_fr_v*cos(pi/4-c_fr_a) , - c_fl_v*cos(-pi/4-c_fl_a), # c_br_v*cos(-pi/4-c_br_a), - c_bl_v*cos(pi/4-c_bl_a) )) # rospy.loginfo("%f %f %f %f" % (cos(pi/4-c_fr_a) , - cos(-pi/4-c_fl_a), # cos(-pi/4-c_br_a), - cos(pi/4-c_bl_a))) curr_angular_z = (c_fr_v * cos(pi / 4 - c_fr_a) - c_fl_v * cos(-pi / 4 - c_fl_a) + c_br_v * cos(-pi / 4 - c_br_a) - c_bl_v * cos(pi / 4 - c_bl_a)) / (4 * offset) # rospy.loginfo("wheel %f %f %f %f" % (c_fr_v, c_fl_v, c_br_v, c_bl_v)) # rospy.loginfo("rotate %f %f %f %f" % (c_fr_a, c_fl_a, c_br_a, c_bl_a)) # rospy.loginfo("curent cmd %f %f %f" % (curr_linear_x, curr_linear_y, curr_angular_z)) delta_x = (curr_linear_x * cos(self.th) - curr_linear_y * sin(self.th)) * control_interval delta_y = (curr_linear_x * sin(self.th) + curr_linear_y * cos(self.th)) * control_interval delta_th = curr_angular_z * control_interval self.x += delta_x self.y += delta_y self.th += delta_th # rospy.loginfo(" -> %f %f %f\n", self.x, self.y, self.th) q = quaternion_about_axis(self.th, (0, 0, 1)) self.odom.pose.pose.position.x = self.x self.odom.pose.pose.position.y = self.y self.odom.pose.pose.position.z = 0 self.odom.pose.pose.orientation.x = q[0] self.odom.pose.pose.orientation.y = q[1] self.odom.pose.pose.orientation.z = q[2] self.odom.pose.pose.orientation.w = q[3] ## self.odom.twist.twist.linear.x = curr_linear_x self.odom.twist.twist.linear.y = curr_linear_y self.odom.twist.twist.angular.z = curr_angular_z if self.publish_odom: self.pub_odom.publish(self.odom) self.last_cmd = self.curr_cmd
def __init__(self): self.mav_name = rospy.get_param('~mav_name', 'mav') # Altitude source ('vicon'/'external') self.altitude_input = rospy.get_param('~altitude_input', 'vicon') # Whether to publish pose message or not (True/False) self.publish_pose = rospy.get_param('~publish_pose', True) # Whether to publish transform message or not (True/False) self.publish_transform = rospy.get_param('~publish_transform', True) # Maximum noise radius (polar coordinates) [m] self.max_noise_radius = rospy.get_param('~max_noise_radius', 0.0) # max tested: 0.15 # Timeout between noise sampling updates [s] self.publish_timeout = rospy.get_param('~publish_timeout', 0.2) self.fix = NavSatFix() self.fix.header.frame_id = 'fcu' self.fix.status.status = 1 self.fix.status.service = 1 self.fix.latitude = 44.0 self.fix.longitude = 44.0 self.fix.altitude = 0.0 # TODO: fill GPS covariance self.latest_altitude_message = Float64() self.latest_imu_message = Imu() self.pwc = PoseWithCovarianceStamped() self.pwc.header.frame_id = 'vicon' # doesn't really matter to MSF self.pwc.pose.covariance[6 * 0 + 0] = 0.000025 self.pwc.pose.covariance[6 * 1 + 1] = 0.000025 self.pwc.pose.covariance[6 * 2 + 2] = 0.000025 self.pwc.pose.covariance[6 * 3 + 3] = 0.000025 self.pwc.pose.covariance[6 * 4 + 4] = 0.000025 self.pwc.pose.covariance[6 * 5 + 5] = 0.000025 self.point = PointStamped() self.point.header = self.pwc.header self.transform = TransformStamped() self.transform.header = self.pwc.header self.child_frame_id = self.mav_name + '_noisy' self.R_noise = 0.0 self.theta_noise = 0.0 self.timer_pub = None self.timer_resample = None self.got_odometry = False self.spoofed_gps_pub = rospy.Publisher('spoofed_gps', NavSatFix, queue_size=1) self.pose_pub = rospy.Publisher('noisy_vicon_pose', PoseWithCovarianceStamped, queue_size=1, tcp_nodelay=True) self.point_pub = rospy.Publisher('noisy_vicon_point', PointStamped, queue_size=1, tcp_nodelay=True) self.transform_pub = rospy.Publisher('noisy_vicon_transform', TransformStamped, queue_size=1, tcp_nodelay=True) self.altitude_sub = rospy.Subscriber('laser_altitude', Float64, self.altitude_callback, tcp_nodelay=True) self.gps_sub = rospy.Subscriber('gps', NavSatFix, self.gps_callback, tcp_nodelay=True) self.estimated_odometry_sub = rospy.Subscriber('estimated_odometry', Odometry, self.odometry_callback, queue_size=1, tcp_nodelay=True) self.imu_sub = rospy.Subscriber('imu', Imu, self.imu_callback, queue_size=1, tcp_nodelay=True)
import rospy from std_msgs.msg import Float64 count = 0 count2 = 0 # -------------------------------Ros Control rospy.init_node('control', anonymous=False) pub_wheel_l_f = rospy.Publisher('/path_planner/left_wheel_f_controller/command', Float64 , queue_size=10,latch=True ) pub_wheel_r_f = rospy.Publisher('/path_planner/right_wheel_f_controller/command', Float64 , queue_size=10,latch=True ) pub_wheel_l_r = rospy.Publisher('/path_planner/left_wheel_r_controller/command', Float64 , queue_size=10,latch=True ) pub_wheel_r_r = rospy.Publisher('/path_planner/right_wheel_r_controller/command', Float64 , queue_size=10,latch=True) print 'initial publish' pub_wheel_l_f.publish(Float64(2)) pub_wheel_r_f.publish(Float64(2)) pub_wheel_l_r.publish(Float64(2)) pub_wheel_r_r.publish(Float64(2)) time.sleep(2) r = np.load('Route_path_2.npy') # print r prev_x,prev_y = r[0] print prev_x,prev_y for x,y in r:
def controller(self, event): with self.js_mutex: js_msg = self.joint_states.position with self.mutex: if ((self.gripper_state == GripperState.OPEN and js_msg[-2] >= self.resp.upper_gripper_limit) or (self.gripper_state == GripperState.CLOSE and js_msg[-2] <= self.resp.lower_gripper_limit)): gripper_cmd = Float64() gripper_cmd.data = 0 self.pub_gripper_command.publish(gripper_cmd) self.gripper_state = GripperState.IDLE if (self.arm_state == ArmState.ROBOT_POSE_CONTROL): for i in range(self.num_joints): self.joint_commands.cmd[i] = self.controllers[ i].compute_control(self.angles[i], js_msg[i]) if (sum(map(abs, self.joint_commands.cmd)) < 0.1): self.arm_state = ArmState.STOPPED if (self.arm_state == ArmState.VELOCITY_IK): # calculate the latest T_sb (transform of the end-effector w.r.t. the 'Space' frame) T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, js_msg[:self.num_joints]) # if the arm is at least 6 dof, then calculate the 'yaw' of T_sy from T_sb # Note that this only works well if the end-effector is not pitched at +/- 90 deg # If the arm is less than 6 dof, then calculate the 'yaw' of T_sy from the 'waist' joint as this will always have the # same 'yaw' as T_sb; there will also not be any issue if the end-effector is pitched at +/- 90 deg if (self.num_joints >= 6): yaw = np.arctan2(T_sb[1, 0], T_sb[0, 0]) else: yaw = js_msg[self.joint_indx_dict["waist"]] self.yaw_to_rotation_matrix(yaw) # Transform T_sb to get the end-effector w.r.t. T_sy T_yb = np.dot(mr.TransInv(self.T_sy), T_sb) # Vy holds the desired twist w.r.t T_sy. This frame is used since its 'X-axis' # is always aligned with the 'X-axis' of the end-effector. Additionally, its # 'Z-axis' always points straight up. Thus, it's easy to think of twists in this frame. Vy = self.twist # If the end-effector is only doing horizontal movement... if (self.velocity_horz_ik_only == True): # Calculate the error in the current end-effector pose w.r.t the initial end-effector pose. err = np.dot(mr.TransInv(T_sb), self.ee_reference) # convert this pose error into a twist w.r.t the end-effector frame Vb_err = mr.se3ToVec(mr.MatrixLog6(err)) # transform this twist into the T_sy frame as this is the frame that the desired twist is in Vy_err = np.dot(mr.Adjoint(T_yb), Vb_err) # if moving in the x-direction, set Vx in Vy_err to 0 and use the Vx value in self.twist instead if (self.twist[3] != 0): Vy_err[3] = 0 # if moving in the y-direction, set Vy in Vy_err to 0 and use the Vy value in self.twist instead if (self.num_joints >= 6 and self.twist[4] != 0): Vy_err[4] = 0 # if moving in the x-y plane, update the 'x' and 'y' values in ee_ref so that Vy_err is calculated correctly if (self.num_joints >= 6 and self.twist[3] != 0 and self.twist[4] != 0): self.ee_reference[0, 3] = T_sb[0, 3] self.ee_reference[1, 3] = T_sb[1, 3] Vy = np.add(Vy_err, self.twist) # The whole process above is done to account for gravity. If the desired twist alone was converted to # joint velocities, over time the end-effector would sag. Thus, by adding in the 'error' twist, the # end-effector will track the initial height much more accurately. # Convert the twist from the T_sy frame to the 'Space' frame using the Adjoint Vs = np.dot(mr.Adjoint(self.T_sy), Vy) # Calculte the Space Jacobian js = mr.JacobianSpace(self.robot_des.Slist, js_msg[:self.num_joints]) # Calculate the joint velocities needed to achieve Vsh qdot = np.dot(np.linalg.pinv(js), Vs) # If any of the joint velocities violate their velocity limits, scale the twist by that amount scaling_factor = 1.0 for x in range(self.num_joints): if (abs(qdot[x]) > self.resp.velocity_limits[x]): sample_factor = abs( qdot[x]) / self.resp.velocity_limits[x] if (sample_factor > scaling_factor): scaling_factor = sample_factor if (scaling_factor != 1.0): qdot[:] = [x / scaling_factor for x in qdot] self.joint_commands.cmd = qdot if (self.arm_state != ArmState.IDLE and self.arm_state != ArmState.STOPPED): self.check_joint_limits() if (self.arm_state == ArmState.STOPPED): # Send a speed of 0 rad/s to each joint - effectively, stopping all joints self.joint_commands.cmd = [0] * self.num_joints # Publish the joint_commands message if (self.arm_state != ArmState.IDLE and self.arm_state != ArmState.SINGLE_JOINT or self.stop_single_joint == True): self.pub_joint_commands.publish(self.joint_commands) if (self.stop_single_joint == True): self.stop_single_joint = False if (self.arm_state == ArmState.STOPPED): self.arm_state = ArmState.IDLE
def go(): s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) s.connect((host, port)) first = True buf = '' count = 0 while True: d = s.recv(2**12) if not d: break buf += d lines = buf.split('\n') buf = lines[-1] for line in lines[:-1]: if first: first = False continue if count % decimation == 0: d = json.loads(line) ecef_cov = numpy.array(d[ 'X_position_relative_position_orientation_ecef_covariance'] ) absodom_pub.publish( Odometry( header=Header( stamp=rospy.Time.from_sec(d['timestamp'] * 1e-9), frame_id='/ecef', ), child_frame_id=child_frame_id, pose=PoseWithCovariance( pose=Pose( position=Point(*d['position_ecef']), orientation=Quaternion( **d['orientation_ecef']), ), covariance=numpy.vstack(( numpy.hstack( (ecef_cov[0:3, 0:3], ecef_cov[0:3, 6:9])), numpy.hstack( (ecef_cov[6:9, 0:3], ecef_cov[6:9, 6:9])), )).flatten(), ), twist=TwistWithCovariance( twist=Twist( linear=Vector3(*d['velocity_body']), angular=Vector3(*d['angular_velocity_body']), ), covariance=numpy.vstack(( numpy.hstack((d['X_velocity_body_covariance'], numpy.zeros((3, 3)))), numpy.hstack( (numpy.zeros((3, 3)), d['X_angular_velocity_body_covariance'])), )).flatten(), ), )) odom_pub.publish( Odometry( header=Header( stamp=rospy.Time.from_sec(d['timestamp'] * 1e-9), frame_id='/enu', ), child_frame_id=child_frame_id, pose=PoseWithCovariance( pose=Pose( position=Point(*d['relative_position_enu']), orientation=Quaternion(**d['orientation_enu']), ), covariance=numpy.array(d[ 'X_relative_position_orientation_enu_covariance'] ).flatten(), ), twist=TwistWithCovariance( twist=Twist( linear=Vector3(*d['velocity_body']), angular=Vector3(*d['angular_velocity_body']), ), covariance=numpy.vstack(( numpy.hstack((d['X_velocity_body_covariance'], numpy.zeros((3, 3)))), numpy.hstack( (numpy.zeros((3, 3)), d['X_angular_velocity_body_covariance'])), )).flatten(), ), )) clock_error_pub.publish(Float64(d['X_clock_error'])) tf_pub.publish( tfMessage(transforms=[ TransformStamped( header=Header( stamp=rospy.Time.from_sec(d['timestamp'] * 1e-9), frame_id='/enu', ), child_frame_id=child_frame_id, transform=Transform( translation=Point(*d['relative_position_enu']), rotation=Quaternion(**d['orientation_enu']), ), ), ], )) count += 1
def callback1(data): wl = (data.axes[1] - data.axes[0]*L/2)/R pub1.publish(Float64(wl))
def callback2(data): wr = (data.axes[1] + data.axes[0]*L/2)/R pub2.publish(Float64(wr))
def emergency_stop(self): stopping_speed = Float64() stopping_speed.data = 0.0 self.speed_command_publisher.publish(stopping_speed)
#!/usr/bin/env python import rospy from std_msgs.msg import Float64 import time import math from geometry_msgs.msg import Point, Twist from nav_msgs.msg import Odometry c = Float64() out = Twist() def publisher(): rospy.init_node('umic_bot', anonymous=True) pub = rospy.Publisher('/mybot/mobile_base_controller/cmd_vel', Twist, queue_size=10) pub2 = rospy.Publisher('/mybot/gripper_extension_controller/command', Float64, queue_size=10) c.data = 0 out.linear.x = 0 out.linear.y = 0 out.linear.z = 0 out.angular.x = 0 out.angular.y = 0 out.angular.z = 0 t0 = 0
def callback(data): """ function docstring, yo! """ msg = Float64() msg.data = data.left_hand.grab_strength*255.0 cmd_grip_pub.publish(msg)
def __init__(self): #======================================================================# # Create Pepper Model for Inverse Kinematics #======================================================================# # Set Joints self.joint_names = [ 'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw' ] self.joint_names_side = { 'L': self.joint_names[0:5], 'R': self.joint_names[5:10] } self.angle_setpoints = {} for key in self.joint_names: self.angle_setpoints[key] = 0.0 # Set bounds for optimization self.bounds_arm = { 'L': [(-2.0857, 2.0857), (0.0087, 1.5620), (-2.0857, 2.0857), (-1.5620, -0.0087)], 'R': [(-2.0857, 2.0857), (-1.5620, -0.0087), (-2.0857, 2.0857), (0.0087, 1.5620)] } self.bounds_wrist = { 'L': [(-1.8239, 1.8239)], 'R': [(-1.8239, 1.8239)] } # These are the standard poses used for starting the optimization self.standard_poses = np.array( [[ -self.bounds_arm['L'][0][0], 0.0, -np.pi / 2.0, -np.pi / 4.0, 0.0, -self.bounds_arm['R'][0][0], 0.0, np.pi / 2.0, np.pi / 4.0, 0.0 ], [ np.pi / 2.0, np.pi / 2.0, -np.pi / 2.0, -0.3, 0.0, np.pi / 2.0, -np.pi / 2.0, np.pi / 2.0, 0.3, 0.0 ], [0.2, 0.2, -1.0, -0.4, 0.0, 0.2, -0.2, 1.0, 0.4, 0.0]]) # Pepper Model Object self.pepper_model = PepperModel() #======================================================================# # ROS Setup #======================================================================# #===== Start Node =====# rospy.init_node('vr_hand_controllers') #===== Parameters =====# # Loop rate self.frequency = 30 self.rate = rospy.Rate(self.frequency) # Ratio of Pepper's arm to human arm self.arm_ratio = rospy.get_param('~arm_ratio', 1.0) rospy.loginfo('[{0}]: Arm ratio: {1}'.format(rospy.get_name(), self.arm_ratio)) self.velocity_linear_max = rospy.get_param('~velocity_linear_max', 0.3) self.velocity_angular_max = rospy.get_param('~velocity_angular_max', 0.4) rospy.loginfo( '[{0}]: Max linear velocity: {1}, Max angular velocity: {2}'. format(rospy.get_name(), self.velocity_linear_max, self.velocity_angular_max)) self.joystick_deadband = rospy.get_param('~joystick_deadband', 0.2) # Name of left controller frame from vive_ros self.left_name = rospy.get_param('~left_name', 'controller_LHR_FFF73D47') # Name of right controller frame from vive_ros self.right_name = rospy.get_param('~right_name', 'controller_LHR_FFFAFF45') # The user must calibrate the yaw offset so that the X axis points in # the foward direction. This is the fixed frame to which the # 'world_rotated' frame will be attached, but with the X axis poining # forward. # NOTE: It is assumed this frame is level with the ground. self.fixed_frame = rospy.get_param('~fixed_frame', 'world') # Sets the joint speed of the arms self.fraction_max_arm_speed = rospy.get_param('~speed_fraction', 0.1) self.calibration_time = rospy.get_param('~calibration_time', 1.0) self.yaw_offset = rospy.get_param('/headset_control/yaw_offset', None) if self.yaw_offset is None: rospy.loginfo( '[{0}]: No yaw_offset parameter found at "/headset_control/yaw_offset". Using value set at "{0}/yaw_offset"' .format(rospy.get_name())) self.yaw_offset = rospy.get_param('~yaw_offset', None) else: # THe headset_control yaw_offset is in the opposite direction as # desired here, so the value must be flipped. self.yaw_offset *= -1 # Error weights used in the optimization self.position_weight = rospy.get_param('~position_weight', 100.0) self.orientation_weight = rospy.get_param('~orientation_weight', 1.0) #=====- TF Listener and Broadcaster =====# self.tfListener = tf.TransformListener() self.tfBroadcaster = tf.TransformBroadcaster() #===== Publishers =====# self.joint_angles_msg = JointAnglesWithSpeed() self.joint_angles_msg.joint_names = self.joint_names self.joint_angles_msg.speed = self.fraction_max_arm_speed self.joint_angles_pub = rospy.Publisher( '/pepper_interface/joint_angles', JointAnglesWithSpeed, queue_size=3) self.cmd_vel_msg = Twist() self.cmd_vel_pub = rospy.Publisher('/pepper_interface/cmd_vel', Twist, queue_size=3) self.left_hand_grasp_msg = Float64() self.left_hand_grasp_pub = rospy.Publisher( '/pepper_interface/grasp/left', Float64, queue_size=3) self.right_hand_grasp_msg = Float64() self.right_hand_grasp_pub = rospy.Publisher( '/pepper_interface/grasp/right', Float64, queue_size=3) #===== Subscribers =====# self.left_controller_sub = rospy.Subscriber('/vive/' + self.left_name + '/joy', Joy, self.leftCallback, queue_size=1) self.right_controller_sub = rospy.Subscriber('/vive/' + self.right_name + '/joy', Joy, self.rightCallback, queue_size=1) #======================================================================# # Controller Setup and Calibration #======================================================================# #===== Controller Transforms =====# self.controller = { 'L': Transform([0., 0., 0.], [0., 0., 0., 1.], 'world_rotated', self.left_name), 'R': Transform([0., 0., 0.], [0., 0., 0., 1.], 'world_rotated', self.right_name) } self.controller_rotated = { 'L': Transform([0., 0., 0.], [0., 0., 0., 1.], 'world_rotated', 'LHand_C'), 'R': Transform([0., 0., 0.], [0., 0., 0., 1.], 'world_rotated', 'RHand_C') } # Multiply the controller rotation by this rotation to adjust it to the # correct frame. # The initial orientation for the controllers are x: right, y: up, z: # backwards. Needed orientation is x: forward, y: left, z: up. self.controller_rotation = tf.transformations.quaternion_from_euler( 0., math.pi / 2, -math.pi / 2, 'rzyx') #===== Yaw_offset calibration =====# # This rotation is applied to the controllers to rotate them about the # world frame to be oriented such that the operator's forward position # is directly along the X axis of the world. This new frame is published # as 'world_rotated' self.world_rotated_position = [0., 0., 0.] self.world_rotated = Transform(self.world_rotated_position, [0., 0., 0., 1.], self.fixed_frame, 'world_rotated') self.running_calibration = False # If the 'headset_control' node has not set the yaw_offset parameter and # it is not provided as a parameter for this node, then it must be # calibrated here. if self.yaw_offset is None: self.yaw_offset = 0 rospy.loginfo( '[{0}]: No yaw_offset parameter found at {0}/yaw_offset. You will need to perform a calibration.' .format(rospy.get_name())) print('\n{0}\n{1}{2}\n{0}'.format(60 * '=', 16 * ' ', 'Orientation Calibration')) print( 'Point both controllers towards the front direction. Press the side button on the LEFT controller and hold for {0} seconds.' .format(self.calibration_time)) # Wait for user to press the side button and for calibration to # complete self.orientation_calibration = False while not self.orientation_calibration: self.rate.sleep() # Publish the new 'world_rotated' frame self.world_rotated.broadcast(self.tfBroadcaster) #===== Hand Origin Calibration =====# # Controller Origin self.controller_origin = {'L': [0., 0., 0.], 'R': [0., 0., 0.]} print('\n{0}\n{1}{2}\n{0}'.format(60 * '=', 16 * ' ', 'Position Calibration')) print( 'Point arms straight towards the ground. Press the side button on the RIGHT controller and hold position for {0} seconds.' .format(self.calibration_time)) # Wait for user to press the side button and for calibration to complete self.position_calibration = False while not self.position_calibration: self.rate.sleep() #======================================================================# # Debugging Frames #======================================================================# # Transform to link Pepper's base_link to the world frame self.base_link = Transform([0., 0., 0.82], [0., 0., 0., 1.], 'world_rotated', 'base_link_V') # DEBUG: Test pose for controllers self.test_pose = { 'L': Transform([0.7, 0.3, 1.2], [-0.7427013, 0.3067963, 0.5663613, 0.1830457], 'world_rotated', 'LHand_C'), 'R': Transform([0.8, -0.3, 1.5], [0.771698, 0.3556943, -0.5155794, 0.1101889], 'world_rotated', 'RHand_C') } # Display the origin and setpoint for human and pepper self.pepper_origin = { 'L': Transform(self.pepper_model.hand_origin['L'], [0., 0., 0., 1.], 'base_link_V', 'LOrigin_P'), 'R': Transform(self.pepper_model.hand_origin['R'], [0., 0., 0., 1.], 'base_link_V', 'ROrigin_P') } self.pepper_setpoint = { 'L': Transform([0., 0., 0.], [0., 0., 0., 1.], 'base_link_V', 'LSetpoint_P'), 'R': Transform([0., 0., 0.], [0., 0., 0., 1.], 'base_link_V', 'RSetpoint_P') } self.human_origin = { 'L': Transform(self.controller_origin['L'], [0., 0., 0., 1.], 'world_rotated', 'LOrigin_H'), 'R': Transform(self.controller_origin['R'], [0., 0., 0., 1.], 'world_rotated', 'ROrigin_H') }
def m6Control(value): data = Float64(value) rospy.sleep(0.1) pub_m6.publish(data)
def __init__(self): pantilt_radian_pub = rospy.Publisher('pantilt_radian_msg', Pantilt, queue_size=10) pantilt_message = Pantilt() rospy.init_node('exp_control', anonymous=True) ##### set initial tilt and pan radian ##### position_1_tilt = 0.0 position_1_pan = 0.0 exp_num = 0 exp_pos = 1 current_pos = 1 pub_pan = rospy.Publisher('pan_controller/command', Float64, queue_size=10) pub_tilt = rospy.Publisher('tilt_controller/command', Float64, queue_size=10) float_pan = Float64() float_tilt = Float64() rospy.set_param("/exp_num", exp_num) rospy.set_param("/exp_pos", exp_pos) while True: a = input("Input exp_num: >>") # print a current_pos = rospy.get_param("exp_pos") rospy.set_param("/exp_miki_img/switch", 0) pub_pan.publish(float_pan) pub_tilt.publish(float_tilt) time.sleep(1) if a == 1 or a == 4 or a == 7 or a == 10: rospy.set_param("/exp_num", a) pantilt_message.speed.x = 0.5 pantilt_message.speed.y = 0.5 pantilt_message.speed.z = 0.0 pantilt_message.position.x = 0.0 pantilt_message.position.y = 1.1 pantilt_message.position.z = 0.0 pantilt_radian_pub.publish(pantilt_message) elif a == 2 or a == 5 or a == 8 or a == 11: rospy.set_param("/exp_num", a) pantilt_message.speed.x = 0.5 pantilt_message.speed.y = 0.5 pantilt_message.speed.z = 0.0 pantilt_message.position.x = -0.78 pantilt_message.position.y = 1.1 pantilt_message.position.z = 0.0 pantilt_radian_pub.publish(pantilt_message) elif a == 3 or a == 6 or a == 9 or a == 12: rospy.set_param("/exp_num", a) pantilt_message.speed.x = 0.5 pantilt_message.speed.y = 0.5 pantilt_message.speed.z = 0.0 pantilt_message.position.x = -2.35 pantilt_message.position.y = 1.1 pantilt_message.position.z = 0.0 pantilt_radian_pub.publish(pantilt_message) elif a == 0: sys.exit() else: pass
def __init__(self): # initial publisher for gripper command topic which is used for gripper control self.pub_gripper = rospy.Publisher("/gripper_joint/command", Float64, queue_size=1) check = True # you need to check if moveit server is open or not while (check): check = False try: # Instantiate a MoveGroupCommander object. This object is an interface to a planning group # In our case, we have "arm" and "gripper" group self.move_group = moveit_commander.MoveGroupCommander("arm") except: print "moveit server isn't open yet" check = True # First initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) # Instantiate a RobotCommander object. # Provides information such as the robot's kinematic model and the robot's current joint states self.robot = moveit_commander.RobotCommander() # We can get the name of the reference frame for this robot: planning_frame = self.move_group.get_planning_frame() print "============ Planning frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = self.move_group.get_end_effector_link() print "============ End effector link: %s" % eef_link # We can get a list of all the groups in the robot: group_names = self.robot.get_group_names() print "============ Available Planning Groups:", group_names # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state", self.robot.get_current_state( ) print "" ### Go strange self.strange() ############################ Method : Using IK to calculate joint value ############################ ### open gripper grip_data = Float64() grip_data.data = 0.5 self.pub_gripper.publish(grip_data) rospy.sleep(2) # After determining a specific point where arm should move, we input x,y,z,degree to calculate joint value for each wrist. pose_goal = Pose() pose_goal.position.x = 0.105 pose_goal.position.y = -0.010 pose_goal.position.z = 0.02 # ik_4dof.ik_solver(x, y, z, degree) joint_value = ik_4dof.ik_solver(pose_goal.position.x, pose_goal.position.y, pose_goal.position.z, -90) for joint in joint_value: joint = list(joint) # determine gripper state joint.append(0) try: self.move_group.go(joint, wait=True) except: rospy.loginfo(str(joint) + " isn't a valid configuration.") # Reference: https://ros-planning.github.io/moveit_tutorials/doc/move_group_python_interface/move_group_python_interface_tutorial.html ### close gripper rospy.sleep(2) grip_data = Float64() grip_data.data = 1.7 self.pub_gripper.publish(grip_data) rospy.sleep(2) ### Go home self.home()
from geomagic_control.msg import DeviceButtonEvent, DeviceFeedback from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint ########################################## # Joint trayectory control UR5e Geomagic # ########################################## ############ # Var init # ############ # Botones boton_gris = Int16() # Forces vars force_x = Float64() force_y = Float64() force_z = Float64() # Math vars pi = math.radians(180) ######################################### # Callback button from /Geomagic/button # ######################################### def Callback_botones(botones): boton_gris.data = botones.grey_button #rospy.loginfo(boton_gris) ############################## # Callback pose from /wrench # ##############################
def set_camera_pan(pub, pan_rad): pan_msg = Float64() pan_msg.data = pan_rad rospy.loginfo('Going to camera pan: {} rad'.format(pan_rad)) pub.publish(pan_msg)
def __init__(self): # Initializing arm interface self._arm_interface = ArmInterface() # PID controllers self._controllers = dict() # Current reference for each joint self._reference_pos = dict() # Output command topics self._command_topics = dict() # Axes mapping self._axes = dict() # Axes gain values self._axes_gain = dict() # Default for the RB button of the XBox 360 controller self._deadman_button = 5 if rospy.has_param('~deadman_button'): self._deadman_button = int(rospy.get_param('~deadman_button')) # If these buttons are pressed, the arm will not move if rospy.has_param('~exclusion_buttons'): self._exclusion_buttons = rospy.get_param('~exclusion_buttons') if type(self._exclusion_buttons) in [float, int]: self._exclusion_buttons = [int(self._exclusion_buttons)] elif type(self._exclusion_buttons) == list: for n in self._exclusion_buttons: if type(n) not in [float, int]: raise rospy.ROSException( 'Exclusion buttons must be an' ' integer index to the joystick button') else: self._exclusion_buttons = list() # Default for the start button of the XBox 360 controller self._home_button = 7 if rospy.has_param('~home_button'): self._home_button = int(rospy.get_param('~home_button')) # Last joystick update timestamp self._last_joy_update = rospy.get_time() # Joystick topic subscriber self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_callback) # Reading the controller configuration controller_config = rospy.get_param('~controller_config') for joint in self._arm_interface.joint_names: for tag in controller_config: if tag in joint: try: # Read the controller parameters self._controllers[joint] = PIDRegulator( controller_config[tag]['controller']['p'], controller_config[tag]['controller']['i'], controller_config[tag]['controller']['d'], 1000) self._command_topics[joint] = rospy.Publisher( controller_config[tag]['topic'], Float64, queue_size=1) self._axes[joint] = controller_config[tag][ 'joint_input_axis'] self._axes_gain[joint] = controller_config[tag][ 'axis_gain'] # Setting the starting reference to the home position # in the robot parameters file self._reference_pos[joint] = deepcopy( self._arm_interface.home[joint]) except: raise rospy.ROSException( 'Error while trying to setup controller for joint <%s>' % joint) rate = rospy.Rate(50) while not rospy.is_shutdown(): pos = self._arm_interface.joint_angles for joint in pos: u = self._controllers[joint].regulate( self._reference_pos[joint] - pos[joint], rospy.get_time()) self._command_topics[joint].publish(Float64(u)) rate.sleep()
#!/usr/bin/env python import rospy from std_msgs.msg import String from std_msgs.msg import Float64 rospy.init_node('my_node') pub = rospy.Publisher('laser_velocity_controller/command', Float64, queue_size=10) rate = rospy.Rate(10) while not rospy.is_shutdown(): msg = Float64() msg.data = 5 pub.publish(msg) rate.sleep()
cartesian_pos = robot_cartesian_pos.data # print("cartesian :", robot_cartesian_pos) #callback function subscribing to ball pose ball_pose = Point() def ball_callback(position_ball): global ball_pose ball_pose = position_ball # print("BALL:", ball_pose) #callback function subscribing to flag if ball is in scene or not ball_in_scene_flag = Float64() def flag_callback(ball_flag): global ball_in_scene_flag ball_in_scene_flag = ball_flag.data # print(ball_in_scene_flag) K = 2 #proportional gain for control velocity rospy.init_node("catch_ball") r = rospy.Rate(60) #subscribers
def twist_lat_control(): rospy.init_node('twist_orange_audi', anonymous=True) rospy.Subscriber(aucmd_topic, Twist, twist_callback) steer_pub.publish(Float64(0.0)) rospy.spin()
def publish(self): # now that these values are published, we # reset the velocity, so that if we don't hear new # ones for the next timestep that we time out; note # that the tire angle will not change # NOTE: we only set self.x to be 0 after 200ms of timeout if rospy.Time.now() - self.lastMsg > self.timeout: rospy.loginfo(rospy.get_caller_id() + " timed out waiting for new input, setting velocity to 0.") self.x = 0 return if self.z != 0: T=self.T L=self.L # self.v is the linear *velocity* r = L/math.fabs(math.tan(self.z)) rL = r-(math.copysign(1,self.z)*(T/2.0)); rR = r+(math.copysign(1,self.z)*(T/2.0)) msgWheelR = Float64() # the right tire will go a little faster when we turn left (positive angle) # amount is proportional to the radius of the outside/ideal msgWheelR.data = self.x*rR/r; msgWheelL = Float64() # the left tire will go a little slower when we turn left (positive angle) # amount is proportional to the radius of the inside/ideal msgWheelL.data = self.x*rL/r; self.pub_wheelL.publish(msgWheelL) self.pub_wheelR.publish(msgWheelR) msgSteerL = Float64() msgSteerR = Float64() # the left tire's angle is solved directly from geometry msgSteerL.data = math.atan2(L,rL)*math.copysign(1,self.z) self.pub_steerL.publish(msgSteerL) # the right tire's angle is solved directly from geometry msgSteerR.data = math.atan2(L,rR)*math.copysign(1,self.z) self.pub_steerR.publish(msgSteerR) else: # if we aren't turning, everything is easy! msgWheel = Float64() msgWheel.data = self.x; self.pub_wheelL.publish(msgWheel) self.pub_wheelR.publish(msgWheel) msgSteer = Float64() msgSteer.data = self.z self.pub_steerL.publish(msgSteer) self.pub_steerR.publish(msgSteer)
def laser_callback(self, msg): pcd = PointCloud() motor_msg = Float64() servo_value = Float64() pcd.header.frame_id = msg.header.frame_id angle = 0 for r in msg.ranges: tmp_point = Point32() tmp_point.x = r * cos(angle) tmp_point.y = r * sin(angle) angle = angle + (1.0 / 180 * pi) if r < 2: pcd.points.append(tmp_point) count_right = 0 count_left = 0 max_front = 0 point_get = [] for pd in range(0, 360): if str(msg.ranges[pd]) == 'inf': point_get.append(float(0.0)) else: point_get.append(float(msg.ranges[pd])) for point in pcd.points: if point.x > 0 and point.x < 0.3 and point.y > 0 and point.y < 1: count_right = count_right + 1 # left if point.x > 0 and point.x < 0.3 and point.y > -1 and point.y < 0: count_left = count_left + 1 # front if point_get[179] > point_get[180] and point_get[179] > point_get[181]: max_front = point_get[179] elif point_get[180] > point_get[179] and point_get[180] > point_get[ 181]: max_front = point_get[180] elif point_get[181] > point_get[179] and point_get[181] > point_get[ 180]: max_front = point_get[181] print("max_front : ", max_front) #servo_value = 0.5304 count = count_left + count_right print("left : ", count_left) print("right : ", count_right) print("count : ", count) print("switch : ", self.switch) #print("stop : ", self.stop) print("abs:", abs(count_left - count_right)) if max_front == 0: motor_msg.data = 0 print("000000000000000000000000000000000000000000000000") if self.switch == 0 and self.stop == 1: print("1") if self.back == 0: servo_value = 0.15 motor_msg.data = 1000 self.left = 1 if 0.3 < max_front < 0.36 and self.left == 1 and self.back == 0: print("back") self.back = 1 if self.back == 1: if max_front < 0.5: servo_value = 1.0 elif max_front > 0.5: self.switch = 1 motor_msg.data = -1000 elif self.switch == 1 and self.stop == 1: print("*****************") print("2") if count_right > 35: print( "comecome!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!1" ) servo_value = 0.5304 elif count_right < 35: servo_value = 0.15 motor_msg.data = 1000 if 0 < max_front < 0.38: print( "stop??????????????????????????????????????????????????????????" ) servo_value = 0.5304 motor_msg.data = 0 self.ck = 1 self.stop = 0 # if back < 5 and count >= 50: # print("back") # back = back + 1 # if count_left > count_right : # servo_value = 0.85 # else : # servo_value = 0.15 # elif back > 5 and count >= 70: # print("back") # back = back + 1 # if count_left > count_right : # servo_value = 0.85 # else : # servo_value = 0.15 # #motor_msg.data = -1000 #if self.stop == 1: # servo_value = 0.5304 #motor_msg.data = 0 print("------") print(motor_msg.data) print(servo_value) self.motor_pub.publish(motor_msg) self.servo_pub.publish(servo_value) self.pcd_pub.publish(pcd)
def request_handler(self, request): if request.data == 'get_distance': dist = self.tof.get_distance() dist_msg = Float64(dist/25.4) self.dist_pub.publish(dist_msg)