def ProcessXlateImage(data):
global PrevDiameter
global p_x
global p_z_ang
global TAG_DIAMETER
global TagFound
global LastTwist
InputTags = data
NewTwist = TwistStamped()
if InputTags.tag_count > 0:
TagFound = True
# +Z_fwd_cam = +Z_base - points up
# +Y_fwd_cam = +Y_base - points left
# +X_fwd_cam = +X_base - points forward
NewTwist.twist.linear.x = InputTags.tags[0].diameter - TAG_DIAMETER
# NewTwist.twist.linear.y = InputTags.tags[0].x - ( FWD_IMAGE_WIDTH/2 ) #( IMAGE_WIDTH/2 ) - InputTags.tags[0].y
# NewTwist.twist.linear.z = InputTags.tags[0].y - ( FWD_IMAGE_HEIGHT/2 ) #( IMAGE_HEIGHT/2 ) - InputTags.tags[0].x
NewTwist.twist.angular.z = InputTags.tags[0].x - (IMAGE_WIDTH / 2) # ( IMAGE_WIDTH/2 ) - InputTags.tags[0].y #
# PrevDiameter = InputTags.tags[0].diameter
# rospy.Publisher("image_pos", NewTwist )
ProcessImagePosition(NewTwist)
# Keep some history for when the tag disappears
while len(PrevVector) >= MAX_HISTORY:
PrevVector.pop(0)
PrevVector.append(NewTwist.twist)
else:
# Extrapolate history
try:
NewTwist.twist = PrevVector.pop(0)
print "Use History %d" % len(PrevVector)
NewTwist.twist = LastTwist
# Save off some history
except IndexError, e:
# Ran out of history
NewTwist.twist.linear.x = 0
NewTwist.twist.linear.y = 0
NewTwist.twist.linear.z = 0
NewTwist.twist.angular.z = 0
if TagFound:
p_x.Reset()
p_z_ang.Reset()
TagFound = False
pub = rospy.Publisher("cmd_vel", Twist)
pub.publish(NewTwist.twist)
def motor_speed_send(a,b,c,d,e,f):
global types_array
pub = rospy.Publisher('robot/desired_velocity', TwistStamped, queue_size=10)
r = rospy.Rate(5)
main = TwistStamped()
stepper = Twist()
stepper.linear.x = a
stepper.linear.y = b
stepper.linear.z = c
stepper.angular.x = d
stepper.angular.y = e
stepper.angular.z = f
main.twist = stepper
now = rospy.get_rostime()
main.header.stamp.secs = now.secs
main.header.stamp.nsecs = now.nsecs
main.header.frame_id = "robot/desired_velocity"
pub.publish(main)
rospy.loginfo("Stepper Motor sent %s", main)
r.sleep()
def pose_callback(data):
# extract x and y position information
x = data.pose.pose.position.x
y = data.pose.pose.position.y
# package and broadcast
p = Point()
p.x = x
p.y = y
# send it
pub_beacon.publish(p)
# and broadbast all odoms on a common channel
pub_odom_all.publish(data)
# publish pose info for viewing
pose_out = PoseStamped()
pose_out.header = data.header
pose_out.header.frame_id = '/world'
pose_out.pose = data.pose.pose
pub_pose.publish(pose_out)
# and twist stamped for DMS interface
vel_out = TwistStamped()
vel_out.header = data.header
vel_out.header.frame_id = '/world'
vel_out.twist = data.twist.twist
pub_vel.publish(vel_out)
# transform
tf_broadcast.sendTransform(
(x, y, 0.0),
(data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w),
rospy.Time.now(), frame_name, "/world")
def publish():
header = Header()
header.stamp = rospy.Time.now()
header.frame_id = 'map'
for role, robot_id in WorldModel.assignments.items():
if robot_id is not None:
command = WorldModel.commands[role]
if command.velocity_control_enable:
msg = TwistStamped()
msg.header = header
msg.twist = command.target_velocity
pubs_velocity[robot_id].publish(msg)
else:
send_pose = WorldModel.tf_listener.transformPose(
"map", command.target_pose)
send_pose.header.stamp = rospy.Time.now()
pubs_position[robot_id].publish(send_pose)
pubs_kick_velocity[robot_id].publish(Float32(command.kick_power))
status = AIStatus()
status.avoidBall = command.avoid_ball
status.avoidDefenceArea = command.avoid_defence_area
status.do_chip = command.chip_enable
status.dribble_power = command.dribble_power
pubs_ai_status[robot_id].publish(status)
command.reset_adjustments()
def pose_callback(data):
# extract x and y position information
x = data.pose.pose.position.x
y = data.pose.pose.position.y
# package and broadcast
p = Point()
p.x = x
p.y = y
# send it
pub_beacon.publish(p)
# and broadbast all odoms on a common channel
pub_odom_all.publish(data)
# publish pose info for viewing
pose_out = PoseStamped()
pose_out.header = data.header
pose_out.header.frame_id = '/world'
pose_out.pose = data.pose.pose
pub_pose.publish(pose_out)
# and twist stamped for DMS interface
vel_out = TwistStamped()
vel_out.header = data.header
vel_out.header.frame_id = '/world'
vel_out.twist = data.twist.twist
pub_vel.publish(vel_out)
# transform
tf_broadcast.sendTransform((x, y, 0.0),
(data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w),
rospy.Time.now(),frame_name,"/world")
def callback(msg):
global pub
output = TwistStamped()
output.header.stamp = rospy.Time.now()
output.header.frame_id = sys.argv[1]
output.twist = msg
pub.publish(output)
def set_velocity(self, value, frame_id=None):
if isinstance(value, TwistStamped):
msg = value
else:
msg = TwistStamped()
msg.header.frame_id = frame_id
if isinstance(value, Twist):
msg.twist = value
else:
if isinstance(value, Vector3):
msg.twist.linear = value
else:
if isinstance(value, np.ndarray):
value = value.reshape((3, 1))
msg.twist.linear.x = value[0, 0]
msg.twist.linear.y = value[1, 0]
msg.twist.linear.z = value[2, 0]
elif isinstance(value, (list, tuple)):
msg.twist.linear.x = value[0]
msg.twist.linear.y = value[1]
msg.twist.linear.z = value[2]
msg.twist.angular.x = 0
msg.twist.angular.y = 0
msg.twist.angular.z = 0
if frame_id is not None:
msg.header.frame_id = frame_id
msg.header.stamp = rospy.Time.now()
self._setpoint_vel = msg
self._setpoint_mode = UAV.SetpointMode.VELOCITY
def onTwist(self,msg): newMsg = TwistStamped() head = std_msgs.msg.Header() newMsg.twist = msg head.stamp = rospy.Time.now() newMsg.header = head self.twist_pub.publish(newMsg)
def handle_convert(data):
pub = rospy.Publisher('/navigation_vel', TwistStamped)
m = TwistStamped()
m.twist = data
m.header = Header()
m.header.stamp = rospy.Time.now()
pub.publish(m)
def set_position():
port = '/dev/ttyACM0'
baud = 115200
ser = serial.Serial(port, baud)
temp = (ser.readline())
a = ser.readline().rstrip()
b = ser.readline().rstrip()
c = ser.readline().rstrip()
d = ser.readline().rstrip()
e = ser.readline().rstrip()
global hold
global holdkm1
global hold_refr
hold = 0
pos = TwistStamped()
pos.twist = Twist()
pos.header = Header()
if int(a) > 1000:
pos.twist.linear.z = 1
print a
elif int(b) > 1000:
pos.twist.linear.x = 0.8
print b
elif int(c) > 1000:
pos.twist.linear.x = -0.8
print c
elif int(d) > 1000:
pos.twist.linear.y = 0.8
print d
elif int(e) > 1000:
pos.twist.linear.y = -.8
print e
return pos
def control_update(self):
control_region = self.valid_state()
if control_region == 1:
v_cmd, w_cmd = self.reachability_control()
elif control_region == 2:
v_cmd, w_cmd = self.forward_control()
elif control_region == 3:
v_cmd, w_cmd = self.right_control()
elif control_region == 4:
v_cmd, w_cmd = self.left_control()
elif control_region == 5:
v_cmd, w_cmd = self.reverse_control()
else:
v_cmd = 0.0
w_cmd = 0.0
v_cmd += compute_reciprocal_avoidance_vel(self.rel_z, 'robot')
self.robot_u = np.array([v_cmd, w_cmd])
control_msg = Twist()
control_msg.linear.x = v_cmd
control_msg.angular.z = w_cmd
self.control_pub.publish(control_msg)
# print current controller region
# print('Current controller region: ' + self.region_map[control_region]),
# print(chr(13)),
# if environment == 'sim':
rosbag_msg = TwistStamped()
rosbag_msg.twist = control_msg
rosbag_msg.header.stamp = rospy.Time.now()
self.rosbag_pub.publish(rosbag_msg)
def control_update(self):
# Read user input from controller
stick_x = self.joy.rightX()
stick_y = self.joy.leftY()
if stick_y >= 0:
v_cmd = stick_y * V_MAX
w_max_v = W_MAX1 + stick_y * (W_MAX2 - W_MAX1)
else:
v_cmd = stick_y * R_MAX
w_max_v = W_MAX1
w_cmd = -stick_x * w_max_v
if self.rel_z is not None:
v_cmd += compute_reciprocal_avoidance_vel(self.rel_z, 'robot')
if environment == 'real':
v_cmd *= REAL_WORLD_FACTOR
w_cmd *= REAL_WORLD_FACTOR
cmd_msg = Twist()
cmd_msg.linear.x = v_cmd
cmd_msg.angular.z = w_cmd
self.cmd_pub.publish(cmd_msg)
# if environment == 'sim':
rosbag_msg = TwistStamped()
rosbag_msg.twist = cmd_msg
rosbag_msg.header.stamp = rospy.Time.now()
self.rosbag_pub.publish(rosbag_msg)
def update(self, state):
if (self.target is None):
rospy.logwarn("Target position for velocity controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
# create output structure
output = TwistStamped()
output.header = state.header
# output velocities
linear = Vector3()
angular = Vector3()
# Control in X vel
linear.x = self.X.update(self.target.position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(self.target.position.y, position.y, time)
# Control in Z vel
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
# TODO
output.twist = Twist()
output.twist.linear = linear
return output
def on_base_velocity_command(data):
t = rospy.get_rostime()
msg = TwistStamped()
msg.header.stamp = t
msg.header.frame_id = 'base'
msg.twist = data
velocity_pub.publish(msg)
def listener_callback(self, inMsg):
outMsg = TwistStamped()
outMsg.header = Header()
outMsg.header.stamp = self.get_clock().now().to_msg()
outMsg.header.frame_id = self.frame_id
outMsg.twist = inMsg
self.publisher_.publish(outMsg)
def outputTwist(self, twist):
g_lateral_accel_limit = 0.8
ERROR = 1e-8
twistCmd = TwistStamped()
twistCmd.twist = twist
twistCmd.header.stamp = rospy.Time.now()
return twistCmd
def update(self, state):
if (self.target is None):
rospy.logwarn("Target position for velocity controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
quat_arr = np.array(
[orientation.x, orientation.y, orientation.z, orientation.w])
att = euler_from_quaternion(quat_arr, 'sxyz')
# create output structure
output = TwistStamped()
output.header = state.header
# output velocities
linear = Vector3()
angular = Vector3()
if (False): #abs(att[2]-self.heading_target) > deg2rad(5.0)):
# Control in X vel
linear.x = self.X.update(position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(position.y, position.y, time)
# Control in Z vel
linear.z = self.Z.update(position.z, position.z, time)
else:
# Control in X vel
linear.x = self.X.update(self.target.position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(self.target.position.y, position.y, time)
# Control in Z vel
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
delta = 0.3
#if((abs(linear.y) > delta) and (abs(linear.x) > delta)) or (self.psi_target == None):
output.twist = Twist()
#if(self.distance(self.target,state.pose)>delta):
phi_target = 0.0
theta_target = 0.0
psi_target = self.heading_target #np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
#psi_target = np.arctan((self.target.position.y-self.prev_target.position.y)/(self.target.position.x-self.prev_target.position.x))#(linear.y,linear.x)
#psi_target = math.atan2(linear.y,linear.x)
#angular.x = self.PHI.update(phi_target, att[0],time)
#angular.y = self.THETA.update(theta_target, att[1],time)
angular.z = self.PSI.update(psi_target, att[2], time)
output.twist.angular = angular
# TODO
output.twist.linear = linear
if output == None:
print "output", output
return output
def callback(cmdVelocity):
baseVelocity = TwistStamped()
baseVelocity.twist = cmdVelocity
now = rospy.get_rostime()
baseVelocity.header.stamp.secs = now.secs
baseVelocity.header.stamp.nsecs = now.nsecs
baseVelocityPub = rospy.Publisher('base_velocity',
TwistStamped,
queue_size=10)
baseVelocityPub.publish(baseVelocity)
rospy.loginfo(rospy.get_caller_id() + "I heard %s", baseVelocity)
def publishCmdVel(cmdvel, velPublisher, velPublisherStamped):
velPublisher.publish(cmdvel)
baseVelocity = TwistStamped()
baseVelocity.twist = cmdvel
now = rospy.get_rostime()
baseVelocity.header.stamp.secs = now.secs
baseVelocity.header.stamp.nsecs = now.nsecs
velPublisherStamped.publish(baseVelocity)
def callback(cmd_velocity):
move_stamped = TwistStamped()
move_stamped.twist = cmd_velocity
now = rospy.get_rostime()
move_stamped.header.stamp.secs = now.secs
move_stamped.header.stamp.nsecs = now.nsecs
move_stampedPub = rospy.Publisher(
'/hummingbird/copilot/manual_desired_velocity',
TwistStamped,
queue_size=10)
move_stampedPub.publish(move_stamped)
def callback(cmdVelocity):
baseVelocity = TwistStamped()
baseVelocity.twist = cmdVelocity
now = rospy.get_rostime()
baseVelocity.header.stamp.secs = now.secs
baseVelocity.header.stamp.nsecs = now.nsecs
baseVelocity.header.frame_id = "map"
baseVelocityPub = rospy.Publisher('base_velocity',
TwistStamped,
queue_size=10)
baseVelocityPub.publish(baseVelocity)
def callback(self, cmdVelocity):
self.baseVelocity.twist = cmdVelocity
self.flag = 1
if self.flag:
# Publish Updated TwistStamped
baseVelocity = TwistStamped()
baseVelocity.twist = cmdVelocity
now = rospy.get_rostime()
baseVelocity.header.stamp.secs = now.secs
baseVelocity.header.stamp.nsecs = now.nsecs
self.baseVelocityPub.publish(baseVelocity)
def parse_twist_stamped(twist_stamped):
rospy.loginfo('twist_stamped')
rospy.logdebug('parse_twist_stamped: parsing ' + str(twist_stamped))
f_from = YamlPars0r.get_dict_value(twist_stamped, 'from')
f_pose = YamlPars0r.get_dict_value(twist_stamped, 'pose')
twist_msg = YamlPars0r.parse_twist(f_pose)
header_msg = Header()
if f_from is not None:
header_msg.frame_id = f_from
twist_stamped_msg = TwistStamped(
header=header_msg
)
if twist_msg is not None:
twist_stamped_msg.twist = twist_msg
return twist_stamped_msg
def getTwistInBaseFrame(self, twistWC, header):
"""Returns a TwistWithCovarianceStamped in base frame"""
# Create the stamped object
twistS = TwistStamped()
twistS.header = header
twistS.twist = twistWC.twist
twistS_base = self.transformTwist(self.base_frame_id, twistS)
twistWC_out = TwistWithCovarianceStamped()
twistWC_out.header = twistS_base.header
twistWC_out.twist.twist = twistS_base.twist
twistWC_out.twist.covariance = twistWC.covariance
return twistWC_out
def cmdVel_publish(self, cmdVelocity):
# Publish Twist
self.cmdVel_pub.publish(cmdVelocity)
# Publish TwistStamped
self.baseVelocity.twist = cmdVelocity
baseVelocity = TwistStamped()
baseVelocity.twist = cmdVelocity
now = rospy.get_rostime()
baseVelocity.header.stamp.secs = now.secs
baseVelocity.header.stamp.nsecs = now.nsecs
self.cmdVelStamped_pub.publish(baseVelocity)
def create_waypoint_message(self, waypoint, reachability_estimator):
lin = Vector3()
angular = Vector3()
# item converts from numpy float type to python float type
lin.x = float(waypoint[0].item()) / 4
lin.y = 0.0
# msg.z = reachability_estimator.item()
lin.z = 0.0
angular.x = 0.0
angular.y = 0.0
angular.z = float(waypoint[1].item()) / 1
msg = TwistStamped()
msg_twist = Twist()
msg_twist.linear = lin
msg_twist.angular = angular
msg.twist = msg_twist
return msg
def update_prop(self, state, gain):
output = TwistStamped()
output.header = state.header
position = state.pose.position
linear = Vector3()
angular = Vector3()
linear.x = gain * (self.target.position.x - position.x)
# Control in Y vel
linear.y = gain * (self.target.position.y - position.y)
# Control in Z vel
linear.z = gain * (self.target.position.z - position.z)
# output velocities
output.twist = Twist()
output.twist.linear = linear
return output
def tf_broadcaster(msg):
global vel_pub
global t
br = tf.TransformBroadcaster()
br.sendTransform(
(msg.pose.position.x, msg.pose.position.y, msg.pose.position.z),
(msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w), rospy.Time.now(),
"base_link", msg.header.frame_id)
br2 = tf.TransformBroadcaster()
br2.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), "map",
msg.header.frame_id)
try:
m = TransformStamped()
m.header.frame_id = "map"
m.header.stamp = rospy.Time.now()
m.child_frame_id = "base_link"
m.transform.translation.x = msg.pose.position.x
m.transform.translation.y = msg.pose.position.y
m.transform.translation.z = msg.pose.position.z
m.transform.rotation.x = msg.pose.orientation.x
m.transform.rotation.y = msg.pose.orientation.y
m.transform.rotation.z = msg.pose.orientation.z
m.transform.rotation.w = msg.pose.orientation.w
t.setTransform(m)
[linear, angular] = t.lookupTwist("base_link", "map", rospy.Time(0.0),
rospy.Duration(1.001))
twist = TwistStamped()
twist.header.stamp = rospy.Time.now()
twist.twist = Twist(Vector3(linear[0], linear[1], linear[2]),
Vector3(angular[0], angular[1], angular[2]))
vel_pub.publish(twist)
except Exception as e:
print 'vel pub error'
def CreateTwistMessage(linear, angular, gt_time, sequence):
msg_av = Vector3()
msg_lv = Vector3()
msg_twist = Twist()
msg_twist_stamped = TwistStamped()
msg_twist_stamped.header.frame_id = "world"
msg_twist_stamped.header.stamp = gt_time
msg_twist_stamped.header.seq = sequence
msg_lv.x = linear.x_val
msg_lv.y = linear.y_val
msg_lv.z = linear.z_val
msg_twist.linear = msg_lv
msg_av.x = angular.x_val
msg_av.y = angular.y_val
msg_av.z = angular.z_val
msg_twist.angular = msg_av
msg_twist_stamped.twist = msg_twist
return msg_twist_stamped
def fix_data(bag_file):
"""
Repairs data in the turtlebot velocity topic by converting to a stamped message.
Both topics (turtlebot velocity and vicon data) are written to a new file with -fixed appended to the name.
Parameters:
bag_file - file to be repaired
Returns:
repaired_file - name of the repaired bag file
"""
# setup repaired file name
repaired_file = bag_file.replace(".bag", "_fixed.bag") # Output bag path
# the two topics to put in the new file
topic1 = '/mobile_base/commands/velocity' # Target topic you want to repair
topic2 = '/vicon/turtlebot_traj_track/turtlebot_traj_track'
# Open bags
bag = rosbag.Bag(bag_file, 'r')
fix_bag = rosbag.Bag(repaired_file, "w") # Create a repaired bag
# Iterate through bag
for topic, msg, t in bag.read_messages():
# Add time back to the target message header
if topic == topic1:
m = TwistStamped()
m.twist = msg
m.header.stamp.secs = t.secs
m.header.stamp.nsecs = t.nsecs
# Write message to bag
fix_bag.write(topic + "_FIXED", m, t)
if topic == topic2:
# Write message to bag
fix_bag.write(topic2, msg, t)
# Close bag - Very important else you'll have to reindex it
fix_bag.close()
return repaired_file
def update(self, state):
if (self.target is None):
rospy.logwarn("Target position for velocity controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
# create output structure
output = TwistStamped()
output.header = state.header
# output velocities
linear = Vector3()
angular = Vector3()
# Control in X vel
linear.x = self.X.update(self.target.position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(self.target.position.y, position.y, time)
# Control in Z vel
#Front sensor
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
(DesRoll, DesPitch,
DesYaw) = tf.transformations.euler_from_quaternion([
self.target.orientation.x, self.target.orientation.y,
self.target.orientation.z, self.target.orientation.w
])
(CurrRoll, CurrPitch,
CurrYaw) = tf.transformations.euler_from_quaternion(
[orientation.x, orientation.y, orientation.z, orientation.w])
angular.z = self.yaw.update(DesYaw, CurrYaw, time)
# TODO
output.twist = Twist()
output.twist.linear = linear
output.twist.angular = angular
return output
def update(self, state, velocity):
self.velocity = velocity
if (self.target is None):
rospy.logwarn("Target position for landing controller is none.")
return None
# simplify variables a bit
time = state.header.stamp.to_sec()
position = state.pose.position
orientation = state.pose.orientation
# create output structure
output = TwistStamped()
output.header = state.header
# check if we're at target altitude to initiate descent
distToTarget = math.pow(
position.x - self.target.position.x, 2) + math.pow(
position.y - self.target.position.y, 2) + math.pow(
position.z - self.target.position.z, 2)
if (not self.landingDescent and distToTarget < self.distTolerance):
self.landingDescent = True
print "Started landing descent"
# output velocities
linear = Vector3()
angular = Vector3()
# Control in X vel
linear.x = self.X.update(self.target.position.x, position.x, time)
# Control in Y vel
linear.y = self.Y.update(self.target.position.y, position.y, time)
# Control in Z vel
if (self.landingDescent):
linear.z = self.descentVelocity
if (not self.descending and velocity.twist.linear.z <
(1 - self.velTolerance) * self.descentVelocity):
self.descending = True
print "Descending"
else:
linear.z = self.Z.update(self.target.position.z, position.z, time)
# Control yaw (no x, y angular)
# TODO
output.twist = Twist()
output.twist.linear = linear
return output
def publish_arm_state():
rospy.wait_for_service('/gazebo/get_link_state')
try:
get_link_state = rospy.ServiceProxy('/gazebo/get_link_state',
GetLinkState)
# Publising arm link poses and twist
ii = 0
for pub in arm_pose_pub_dic:
ii += 1
if ii == 1:
response = get_link_state('Link_1', 'link_chassis')
else:
response = get_link_state('Link_' + str(ii),
'Link_' + str(ii - 1))
link_pose = response.link_state.pose
stamped_link_pose = PoseStamped()
stamped_link_pose.pose = response.link_state.pose
stamped_link_pose.header.stamp = rospy.Time.now(
) # Add a time stamp to the message
pub.publish(stamped_link_pose)
# Publish arm link twists
ii = 0
for pub in arm_twist_pub_dic:
ii += 1
if ii == 1:
response = get_link_state('Link_1', 'link_chassis')
else:
response = get_link_state('Link_' + str(ii),
'Link_' + str(ii - 1))
stamped_link_twist = TwistStamped()
stamped_link_twist.twist = response.link_state.twist
stamped_link_twist.header.stamp = rospy.Time.now(
) # Add a time stamp to the message
pub.publish(stamped_link_twist)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
def twist_cb(self, twist):
# get teleop twist message
twist_stamped = TwistStamped()
twist_stamped.twist = twist
if not (
twist.linear.x == 0
and twist.linear.y == 0
and twist.linear.z == 0
and twist.angular.x == 0
and twist.angular.y == 0
and twist.angular.z == 0
):
# give it a header
header = Header()
header.stamp = rospy.Time.now()
twist_stamped.header = header
# publish the command
self.teleop_stamped_pub.publish(twist_stamped)
def main():
global sub, pub, velo_unity_pub, cur_velo, finger_unity_pub, open
rospy.init_node("prime_kinova_unity_control")
#init Subscriber and Publisher
sub = rospy.Subscriber("GlovesData", GlovesData, callBack)
velo_unity_pub = rospy.Publisher(
'/servo_server/delta_twist_cmds', TwistStamped,
queue_size=1) #j2n6s300_driver/in/cartesian_velocity
finger_unity_pub = rospy.Publisher('/j2n6s300/fingers',
std_msgs.msg.Float32,
queue_size=1)
# rospy.spin()
r = rospy.Rate(110)
while not rospy.is_shutdown():
cur_velo = TwistStamped()
cur_velo.twist = twist
velo_unity_pub.publish(cur_velo)
x = std_msgs.msg.Float32()
x.data = open # 0为close, 1为open
finger_unity_pub.publish(x)
r.sleep()
def process_joy_data_callback(data): linear_x = data.axes[1] * max_linear_velocity linear_y = data.axes[0] * max_linear_velocity angular_z = data.axes[3] * max_angular_velocity seq = data.header.seq twist_stamped_msg = TwistStamped() twist_stamped_msg.twist = Twist() twist_stamped_msg.twist.linear = Vector3() twist_stamped_msg.twist.angular = Vector3() twist_stamped_msg.twist.linear.x = linear_x twist_stamped_msg.twist.linear.y = linear_y twist_stamped_msg.twist.linear.z = 0 twist_stamped_msg.twist.angular.x = 0 twist_stamped_msg.twist.angular.y = 0 twist_stamped_msg.twist.angular.z = angular_z twist_stamped_msg.header.seq = seq twist_stamped_msg.header.frame_id = '/base_link' twist_stamped_msg.header.stamp = data.header.stamp pub.publish(twist_stamped_msg)
def timer_cb(self, event):
enable = False
twist = Twist()
fx = self.cur_wrench.force.x
fy = self.cur_wrench.force.y
fz = self.cur_wrench.force.z
if fx > self.dz_x_max:
twist.linear.x = self.k_f * (fx - self.dz_x_max)
enable = True
elif fx < self.dz_x_min:
twist.linear.x = self.k_f * (fx - self.dz_x_min)
enable = True
if fy > self.dz_y_max:
twist.linear.y = self.k_f * (fy - self.dz_y_max)
enable = True
elif fy < self.dz_y_min:
twist.linear.y = self.k_f * (fy - self.dz_y_min)
enable = True
if fz > self.dz_z_max:
twist.linear.z = self.k_f * (fz - self.dz_z_max)
enable = True
elif fz < self.dz_z_min:
twist.linear.z = self.k_f * (fz - self.dz_z_min)
enable = True
if enable:
twist.linear.x = self.saturate_vel(twist.linear.x)
twist.linear.y = self.saturate_vel(twist.linear.y)
twist.linear.z = self.saturate_vel(twist.linear.z)
twist_s = TwistStamped()
twist_s.header.stamp = rospy.get_rostime()
twist_s.twist = twist
self.pub_cmd_vel.publish(twist_s)
def get_velocity(self, pose): velocity = TwistStamped() velocity.header.stamp = pose.header.stamp velocity.header.frame_id = pose.header.frame_id velocity.twist = pose.twist.twist return velocity
def callback(data): data_new = TwistStamped() data_new.header.stamp = rospy.Time.now() data_new.twist = data pub2.publish(data_new)
def ProcessXlateImage( data ):
global PrevDiameter
global p_x
global p_y
global p_z
global p_z_ang
global FwdCam
global prev_key
global FWD_TAG_DIAMETER
global DWN_TAG_DIAMETER
global TagFound
global NavTwist
global LastTwist
InputTags = data
NewTwist = TwistStamped()
if InputTags.image_width != 320 and FwdCam and prev_key != 'switch':
print "Switch to Down Cam"
p_x.ReInit('vel_x', DWN_X_VEL_SCALAR, DWN_X_INT_SCALAR, DWN_X_DERIVATIVE_SCALAR, DWN_X_MAX_VELOCITY, 0, DWN_X_DEAD_BAND)
p_y.ReInit('vel_y', DWN_Y_VEL_SCALAR, DWN_Y_INT_SCALAR, DWN_Y_DERIVATIVE_SCALAR, DWN_Y_MAX_VELOCITY, 0, DWN_Y_DEAD_BAND)
p_z.ReInit('vel_z', DWN_Z_VEL_SCALAR, DWN_Z_INT_SCALAR, DWN_Z_DERIVATIVE_SCALAR, DWN_Z_MAX_VELOCITY, 0, DWN_Z_DEAD_BAND)
p_z_ang.ReInit('ang_z', DWN_Z_ANG_VEL_SCALAR, DWN_Z_ANG_INT_SCALAR, DWN_Z_ANG_DERIVATIVE_SCALAR, DWN_Z_ANG_MAX_VELOCITY, 0, DWN_Z_ANG_DEAD_BAND)
FwdCam = False
if InputTags.tag_count > 0:
TagFound = True
if InputTags.tags[0].id == 0:
NewTwist.header.frame_id = 'switch'
else:
NewTwist.header.frame_id = 'move'
if FwdCam:
# Forward Camera
# +Z_fwd_cam = +Z_base - points up
# +Y_fwd_cam = +Y_base - points left
# +X_fwd_cam = +X_base - points forward
NewTwist.twist.linear.x = InputTags.tags[0].diameter - FWD_TAG_DIAMETER
NewTwist.twist.linear.y = InputTags.tags[0].x - ( FWD_IMAGE_WIDTH/2 ) #( IMAGE_WIDTH/2 ) - InputTags.tags[0].y
NewTwist.twist.linear.z = InputTags.tags[0].y - ( FWD_IMAGE_HEIGHT/2 ) #( IMAGE_HEIGHT/2 ) - InputTags.tags[0].x
NewTwist.twist.angular.z = 0 # No rotation on fwd cam
#PrevDiameter = InputTags.tags[0].diameter
else:
# Downward Camera
# NOTE: the downward camera
# +Z_down_cam = -Z_base - points down,
# +Y_down_cam = +X_base - points forward
# +X_down_cam = +Y_base - points left
NewTwist.twist.linear.x = InputTags.tags[0].y - ( DWN_IMAGE_HEIGHT/2 )
NewTwist.twist.linear.y = InputTags.tags[0].x - ( DWN_IMAGE_WIDTH/2 )
NewTwist.twist.linear.z = DWN_TAG_DIAMETER - InputTags.tags[0].diameter
print InputTags.tags[0].zRot
NewTwist.twist.angular.z = 2*math.pi - InputTags.tags[0].zRot
#rospy.Publisher("image_pos", NewTwist )
ProcessImagePosition( NewTwist )
# Keep some history for when the tag disappears
while len(PrevVector) >= MAX_HISTORY:
PrevVector.pop(0)
PrevVector.append(NewTwist.twist)
else:
NewTwist.header.frame_id = 'lost'
# Extrapolate history
try:
NewTwist.twist = PrevVector.pop(0)
print "Use History %d" % len(PrevVector)
NewTwist.twist = LastTwist
# Save off some history
except IndexError, e:
# Ran out of history
NewTwist.twist.linear.x = 0
NewTwist.twist.linear.y = 0
NewTwist.twist.linear.z = NavTwist.linear.z
NewTwist.twist.angular.z = 0
if TagFound:
p_x.Reset()
p_y.Reset()
p_z.Reset()
p_z_ang.Reset()
prev_key = 'foobar'
TagFound = False
pub = rospy.Publisher("cmd_vel", Twist )
pub.publish( NewTwist.twist )
def onTwistStamped(self,msg): out_msg = TwistStamped() out_msg.header = rospy.time.now() out_msg.twist = msg self.twist_pub.publish(out_msg)
def main():
if not len(sys.argv) == 2:
print "Usage: ./post_process.py <input.bag>"
sys.exit(1)
topics=["/bebop/states/ARDrone3/PilotingState/AttitudeChanged", "/bebop/cmd_vel", "/vicon/bebop_blue_en/bebop_blue_en", "/bebop/states/ARDrone3/PilotingState/SpeedChanged", "/vservo/cmd_vel"]
input_bag = sys.argv[1]
output_bag = os.path.splitext(os.path.basename(input_bag))[0] + "_processed.bag"
cmd_vel_stamped = TwistStamped()
cmd_vel_stamped.header.frame_id = "base_link"
print "Output: ", output_bag
i = 0
try:
i_bag = rosbag.Bag(input_bag)
o_bag = rosbag.Bag(output_bag, "w")
total_msgs = i_bag.get_message_count(topics)
for topic, msg, t in i_bag.read_messages(topics):
o_bag.write(topic, msg, t)
if topic == "/bebop/cmd_vel":
cmd_vel_stamped.header.stamp = t
cmd_vel_stamped.twist = msg
o_bag.write("/bebop/cmd_vel_stamped", cmd_vel_stamped, t)
i += 1
progress(i, total_msgs, output_bag)
finally:
o_bag.close()
i_bag.close()