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)
