def execute(self, userdata):
self.time_init=rospy.get_rostime()
self.face_data=FaceDetections()
self.subs=rospy.Subscriber(topic_name, FaceDetections, self.callback_topic)
while (len(self.face_data.faces) == 0) and (rospy.get_rostime().secs - self.time_init.secs) < self.topic_time_out:
rospy.sleep(0.3)
if self.preempt_requested():
return 'preempted'
self.subs.unregister()
rospy.loginfo('Faces after Unregistering:: ' + str(self.face_data))
if len(self.face_data.faces) > 0 :
userdata.topic_output_msg = copy.deepcopy(self.face_data)
userdata.standard_error = ''
rospy.logwarn('Face Topic Reader: Faces Detected')
rospy.logwarn('Face Detected:: ' + str(self.face_data))
return 'succeeded'
else :
#time out
userdata.standard_error = "Topic Reader : TimeOut Error"
userdata.topic_output_msg = ''
rospy.logwarn('Face Topic Reader: Time Out Error')
return 'aborted'
def complete_timing_trials():
rospy.init_node('time_trials')
times = dict(zip(['pickup', 'place'], [[], []]))
awesome = pick_and_place()
for i in range(5):
#raw_input("pick up object?")
start = rospy.get_rostime().to_sec()
result = awesome.pick_up()
while not result:
result = awesome.pick_up()
continue
end = rospy.get_rostime().to_sec()
times['pickup'].append(int(end - start)+1) # ceiling round off
#raw_input("place object?")
rospy.sleep(1) # wait for robot to see AR tag
start = rospy.get_rostime().to_sec()
result = awesome.place(use_offset=False)
while not result:
result = awesome.place()
end = rospy.get_rostime().to_sec()
times['place'].append(int(end - start)+1) # ceiling round off
with open("time_results.csv", "wb") as f:
for key in times:
f.write( "%s, %s\n" % (key, ", ".join(repr(e) for e in times[key])))
def onMessaged(linkstates):
global listener
global broadcaster
global lastupdate
if (rospy.get_rostime() - lastupdate).to_sec() < 0.03:
return
lastupdate = rospy.get_rostime()
lastUpdateTime = rospy.get_rostime()
for (link_idx, link_name) in enumerate(linkstates.name):
robot_name = link_name.partition('::')[0]
part_name = link_name.partition('::')[2]
quat = linkstates.pose[link_idx].orientation
pos = linkstates.pose[link_idx].position
#rospy.loginfo(part_name)
if listener.frameExists(part_name):
#rospy.loginfo('stuff')
broadcaster.sendTransform((pos.x,pos.y,pos.z),
(quat.x,quat.y,quat.z,quat.w),
rospy.get_rostime(),
'/'+part_name,
'world')
if listener.frameExists(robot_name+'/'+part_name):
#rospy.loginfo('otherstuff')
broadcaster.sendTransform((pos.x,pos.y,pos.z),
(quat.x,quat.y,quat.z,quat.w),
rospy.get_rostime(),
'/'+robot_name+'/'+part_name,
'world')
def goal_pose_cb(self, data):
rospy.loginfo('Position command: pos={}, speed={}'.format(data.pos, data.speed))
multiplier = 1000
move_cmd = JointState()
move_cmd.name = [self.gripper_name]
move_cmd.position = [data.pos / multiplier]
move_cmd.velocity = [abs(data.speed) / multiplier]
if move_cmd.position[0] < self.js.position[self.link_id]:
move_cmd.velocity = [x * -1 for x in move_cmd.velocity]
rate = rospy.Rate(10)
start_time = rospy.get_rostime()
while not rospy.is_shutdown() and abs(self.js.position[self.link_id] - move_cmd.position[0]) > .007:
self.boxy_cmd.publish(move_cmd)
rate.sleep()
if rospy.get_rostime() - start_time > rospy.Duration(10):
rospy.logwarn('movement took too long, stopping.')
break
move_cmd.velocity = [0.0 for _ in move_cmd.velocity]
move_cmd.effort = [0.0 for _ in move_cmd.velocity]
srv_cmd = SetJointStateRequest()
srv_cmd.state = move_cmd
# print('send {}'.format(srv_cmd))
self.boxy_joint_state.call(srv_cmd)
# self.boxy_joint_state(srv_cmd)
rospy.loginfo('done.')
def send_command(self, command):
"""Send command to the robot.
@param command: command to send to robot (tuple)
returns: respons from robot (tuple)
"""
#msg = ','.join(map(str, command))
msg = ','.join(str(x) for x in command) + '\r'
#rospy.loginfo("msg=%s", str(msg))
if not self.fake_connection:
done = False
start_time = rospy.get_rostime()
while not done:
try:
if self.sci is None:
self.sci = SerialInterface(self.tty, self.baudrate)
with self.sci.lock:
self.sci.flush_input()
self.sci.send(msg)
rsp = self.sci.read()
#rsp = msg
#rospy.loginfo("rsp=%s", str(rsp))
done = True
except Exception, err:
rospy.logerr(str(err))
if rospy.get_rostime() - start_time > self.connection_timeout:
raise rospy.ROSInterruptException("connection to robot seems broken")
self.sci = None # assume serial interface is broken
rospy.loginfo('waiting for connection')
rospy.sleep(1.) # wait 1 seconds before retrying
def handle_multi_range_message(self, multi_range_msg):
"""Handle a ROS multi-range message by updating and publishing the state.
Args:
multi_range_msg (uwb.msg.UWBMultiRangeWithOffsets): ROS multi-range message.
"""
# Update tracker position based on time-of-flight measurements
new_estimate = self.update_estimate(multi_range_msg)
if new_estimate is None:
rospy.logwarn('Could not compute initial estimate: address={}, remote_address={}'.format(
multi_range_msg.address, multi_range_msg.remote_address))
else:
# Publish tracker message
ros_msg = uwb.msg.UWBTracker()
ros_msg.header.stamp = rospy.get_rostime()
ros_msg.address = multi_range_msg.address
ros_msg.remote_address = multi_range_msg.remote_address
ros_msg.state = new_estimate.state
ros_msg.covariance = np.ravel(new_estimate.covariance)
self.uwb_pub.publish(ros_msg)
# Publish target transform (rotation is identity)
self.tf_broadcaster.sendTransform(
(new_estimate.state[0], new_estimate.state[1], new_estimate.state[2]),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.get_rostime(),
self.target_frame,
self.tracker_frame
)
def move_to_synchro_pos(self):
self.init_conman()
self.conmanSwitch([], ['Irp6ptfgSplineTrajectoryGeneratorMotor','Irp6pmSplineTrajectoryGeneratorMotor','Irp6pmSplineTrajectoryGeneratorJoint','Irp6pmPoseInt','Irp6pmForceControlLaw','Irp6pmForceTransformation'], True)
self.conmanSwitch(['Irp6pmSplineTrajectoryGeneratorMotor'], [], True)
self.client = actionlib.SimpleActionClient('/irp6p_arm/spline_trajectory_action_motor', FollowJointTrajectoryAction)
self.client.wait_for_server()
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
goal.trajectory.points.append(JointTrajectoryPoint([0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(10.0)))
goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.2)
self.client.send_goal(goal, self.irp6pm_done_callback)
self.conmanSwitch(['Irp6ptfgSplineTrajectoryGeneratorMotor'], [], True)
self.client_tfg = actionlib.SimpleActionClient('/irp6p_tfg/spline_trajectory_action_motor', FollowJointTrajectoryAction)
self.client_tfg.wait_for_server()
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = ['joint1']
goal.trajectory.points.append(JointTrajectoryPoint([0.0], [0.0], [], [], rospy.Duration(10.0)))
goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.2)
self.client_tfg.send_goal(goal, self.irp6ptfg_done_callback)
self.status.motion_in_progress = True
self.change_motors_widget_state()
def Control12(self):
t= 0.0
dt= self.time_step
#base= self.joint_positions[1]
base= [0.0]*7
cmd= trajectory_msgs.msg.JointTrajectory()
cmd.header.stamp= self.last_l_arm_controller_state.header.stamp
cmd.joint_names= self.joint_names[1]
cmd.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
if self.over_target:
cmd.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
while t<3.14159*4.0:
t= t+dt
cmd.points[0].time_from_start= rospy.Duration(dt)
cmd.points[0].positions= base + np.array([self.Curve(t)]*7)
#cmd.points[0].velocities= [0.0]*7
cmd.points[0].velocities= [self.CurveD(t)]*7
if self.over_target:
cmd.points[1].time_from_start= rospy.Duration(dt*2)
cmd.points[1].positions= base + np.array([self.Curve(t+dt)]*7)
#cmd.points[1].velocities= [0.0]*7
cmd.points[1].velocities= [self.CurveD(t+dt)]*7
self.l_pub.publish(cmd)
start_time= rospy.get_rostime()
while rospy.get_rostime() < start_time + rospy.Duration(dt):
time.sleep(dt*0.1)
def new_diagnostics(self, diag):
for status in diag.status:
if(status.name == "//base_controller"):
if(status.level != 0):## && self.last_base_diag == 0):
self.diag_err = True
elif(status.level == 0):## && self.last_base_diag == 1):
self.diag_err = False
if((rospy.get_rostime() - self.last_led).to_sec() > 0.5):
self.last_led = rospy.get_rostime()
#Trigger LEDS
if(self.diag_err):
if(self.color != "red"):
sss.set_light("red")
self.color = "red"
else:
if ((rospy.get_rostime() - self.last_vel).to_sec() > 1.0):
if(self.color != "green"):
sss.set_light("green")
self.color = "green"
else:
if(self.on):
self.on = False
if(self.color != "yellow"):
sss.set_light("yellow")
self.color = "yellow"
else:
self.on = True
if(self.color != "led_off"):
sss.set_light("led_off")
self.color = "led_off"
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def run(self):
self._stop = False
start = rospy.get_rostime()
rospy.sleep(0.5)
twist = self.twist
max_rotate_count = self.max_rotate_count
rotate_count = max_rotate_count
yaw_rate = self.yaw_rate
while not rospy.is_shutdown() and not self._stop:
if rotate_count == max_rotate_count:
if twist.angular.z > 0:
mod = -1.0
else:
mod = 1.0
update = mod*yaw_rate/10.0
while math.fabs(twist.angular.z) <= yaw_rate:
twist.angular.z = twist.angular.z + update
self.pub_cmd.publish(twist)
rospy.sleep(0.04)
# Make sure it is exact so the inequality in the while loop doesn't mess up next time around
twist.angular.z = mod*yaw_rate
rotate_count = 0
else:
rotate_count += 1
now = rospy.get_rostime()
msg = "Rotate: " + str(now.secs - start.secs)
self.log(msg)
self.pub_cmd.publish(twist)
self.rate.sleep()
def nSteps(self, nL, nR): if nL == 0 and nR == 0: return None P = 150.0 R = 0.09 N = 500 t_sim = (1.0*max(abs(nL),(nR)))/P trcs = 0.4 aux = (1.0*min(abs(nL),abs(nR)))/(1.0*max(abs(nR),abs(nL))) if abs(nL)>=abs(nR): VL = math.copysign(1.0,nL) * P * 2.0 * math.pi * R / N VR = math.copysign(1.0,nR) * abs(VL) * aux else: VR = math.copysign(1.0,nR) * P * 2.0 * math.pi * R / N VL = math.copysign(1.0,nL) * abs(VR) * aux v_lin = (VL + VR)/2.0 v_ang = (VR - VL)/trcs tm = rospy.get_rostime() while tm.secs == 0 and tm.nsecs == 0: tm = rospy.get_rostime() while not rospy.is_shutdown(): cur = rospy.get_rostime() self.cmd.linear.x = v_lin self.cmd.angular.z = v_ang self.cmd_pub.publish(self.cmd) self.rate.sleep() if not (cur - tm) < rospy.Duration(t_sim): break
def Control11(self):
t= 0.0
dt= self.time_step
#base= self.joint_positions[1]
base= np.array([0.0]*7)
goal= pr2_controllers_msgs.msg.JointTrajectoryGoal()
goal.trajectory.header.stamp= rospy.Time.now()
goal.trajectory.joint_names= self.joint_names[1];
for i in range(self.num_points):
goal.trajectory.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
g_prev= base.copy()
while t<3.14159*4.0:
traj_duration= dt
t= t+dt
g= np.array([self.Curve(t)]*7)
#g[4]= AngleMod1(40.0*(g[4]))
dg= np.array([self.CurveD(t)]*7)
#InterpolateLinearly1(goal.trajectory.points, self.joint_positions[1],self.joint_velocities[1], g,dg, dt, self.num_points)
if False:
InterpolateLinearly1(goal.trajectory.points, g_prev, np.array([self.CurveD(t-dt)]*7), g, np.array([self.CurveD(t)]*7), dt, self.num_points)
else:
traj_duration= InterpolateLinearly2(goal.trajectory.points, g_prev, g, dt, dt/float(self.num_points), True, self.vel_limits)
#print goal.trajectory.points
for p in goal.trajectory.points:
self.fp_goal.write('%f %s %s\n' % ( (rospy.get_rostime()-self.ljscallback_start_time+p.time_from_start).to_sec(), ' '.join(map(str,p.positions)), ' '.join(map(str,p.velocities)) ))
#self.fp_goal.write('%f %s %s\n' % ( (rospy.get_rostime()-self.ljscallback_start_time).to_sec(), ' '.join(map(str,g)), ' '.join(map(str,dg)) ))
goal.trajectory.header.stamp= rospy.Time.now()
self.traj_client.send_goal(goal)
#self.traj_client.wait_for_result()
start_time= rospy.get_rostime()
while rospy.get_rostime() < start_time + rospy.Duration(traj_duration):
time.sleep(traj_duration*0.01)
g_prev= g
def calc_triangle(self):
# transform for the first corner of triangle
fst_pt = PointStamped()
fst_pt.header.frame_id = 'base_link'
fst_pt.point.x = 1
fst_pt.point.y = .5
fst_pt.header.stamp = rospy.get_rostime()
try:
self.tf_listener.waitForTransform('base_link', # from here
'map', # to here
fst_pt.header.stamp,
rospy.Duration(1.0))
fst_pt = self.tf_listener.transformPoint('map', fst_pt)
except tf.Exception as e:
rospy.loginfo(e)
# transform for the second corner of triangle
snd_pt = PointStamped()
snd_pt.header.frame_id = 'base_link'
snd_pt.point.x = 1
snd_pt.point.y = -.5
snd_pt.header.stamp = rospy.get_rostime()
try:
self.tf_listener.waitForTransform('base_link', # from here
'map', # to here
snd_pt.header.stamp,
rospy.Duration(1.0))
snd_pt = self.tf_listener.transformPoint('map', snd_pt)
except tf.Exception as e:
rospy.loginfo(e)
return [fst_pt.point.x, fst_pt.point.y, snd_pt.point.x, snd_pt.point.y]
def publish(self, data):
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def increment_array_callback(msg):
print('Received array ' + str(msg.data))
msg = get_joint_velocity_msg(msg.data, rospy.get_rostime())
velocity_publisher.publish(msg)
rospy.sleep(1./rate)
msg = get_joint_velocity_msg([0, 0, 0, 0, 0, 0, 0], rospy.get_rostime())
velocity_publisher.publish(msg)
def run(self):
self._stop = False
start = rospy.get_rostime()
rospy.sleep(0.5)
twist = self.twist
freq = self.freq
self.rate = rospy.Rate(freq)
a = self.accl
max_speed = self.max_speed
msg = "Time : " + str(rospy.get_rostime().secs) + " Vel : " +str(twist.linear.x)
finish = False
while not rospy.is_shutdown() and not self._stop:
if not finish:
twist.linear.x = twist.linear.x + ( 1.0 / freq ) * a
if twist.linear.x > max_speed:
finish = True
twist.linear.x = 0.0
msg = "Time : " + str(rospy.get_rostime().secs) + " Vel : " +str(twist.angular.z)
self.log(msg)
self.pub_cmd.publish(twist)
self.rate.sleep()
twist.linear.x = 0
self.pub_cmd.publish(twist)
def __init__(self):
rospy.init_node('seniorcar_odometry', anonymous=True)
rospy.Subscriber("seniorcar_state", SeniorcarState, self.update_odometry)
rospy.Subscriber('imu/data', Imu, self.update_pitch_roll)
self.pub = rospy.Publisher('seniorcar_odometry',Odometry, queue_size=1000)
self.current_time = rospy.get_rostime()
self.last_time = rospy.get_rostime()
self.odometry.header.frame_id = "odom"
self.odometry.child_frame_id = "base_link"
self.odometry.pose.covariance = COV_MATRIX
self.odometry.twist.covariance = COV_MATRIX
self.odometry.pose.pose.position.x = 0
self.odometry.pose.pose.position.y = 0
self.odometry.pose.pose.position.z = 0
self.odometry.pose.pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, 0))
global CALCULATE_3DIMENTION_ODOMETRY
global STEER_ANGLE_OFFSET
global IMU_PITCH_DEFFULT_ANGLE
global IMU_ROLL_DEFAULT_ANGLE
CALCULATE_3DIMENTION_ODOMETRY = rospy.get_param('~calculate_3dimention_odmetry',CALCULATE_3DIMENTION_ODOMETRY)
STEER_ANGLE_OFFSET = rospy.get_param('~steer_angle_offset',STEER_ANGLE_OFFSET)
IMU_PITCH_DEFFULT_ANGLE = rospy.get_param('~imu_pitch_offset',IMU_PITCH_DEFFULT_ANGLE) * math.pi / 180.0
IMU_ROLL_DEFAULT_ANGLE= rospy.get_param('~imu_roll_offset',IMU_ROLL_DEFAULT_ANGLE) * math.pi / 180.0
def main():
pub_plus = rospy.Publisher('~plus_rect_event', MouseEvent, queue_size=1)
pub_minus = rospy.Publisher('~minus_rect_event', MouseEvent, queue_size=1)
width = rospy.get_param('~image_width')
height = rospy.get_param('~image_height')
plus_events = [
MouseEvent(type=3, x=width/4, y=height/4, width=width, height=height),
MouseEvent(type=4, x=width/2, y=height/2, width=width, height=height),
MouseEvent(type=2, x=3*width/4, y=3*height/4, width=width, height=height),
]
minus_events = [
MouseEvent(type=3, x=3*width/4, y=3*height/4, width=width, height=height),
MouseEvent(type=4, x=width/2, y=height/2, width=width, height=height),
MouseEvent(type=2, x=width/4, y=height/4, width=width, height=height),
]
rate = rospy.Rate(10)
while not rospy.is_shutdown():
for e in plus_events:
e.header.stamp = rospy.get_rostime()
pub_plus.publish(e)
rate.sleep()
for e in minus_events:
e.header.stamp = rospy.get_rostime()
pub_minus.publish(e)
rate.sleep()
def execute(self, userdata):
goal = SingleJointPositionGoal()
goal.position = userdata.position
rospy.loginfo("Sending torso goal with position %0.3f and waiting for result"%userdata.position)
# send the goal
self.torso_client.send_goal(goal)
start_time = rospy.get_rostime()
r = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.get_rostime()
if now - start_time > rospy.Duration(self.torso_timeout):
rospy.loginfo("torso timed out!")
self.torso_client.cancel_goal()
return 'failed'
if self.preempt_requested():
rospy.loginfo("torso goal preempted!")
self.torso_client.cancel_goal()
self.service_preempt()
return 'preempted'
state = self.torso_client.get_state()
if state == GoalStatus.SUCCEEDED:
break
r.sleep()
rospy.loginfo("move torso succeeded")
return 'succeeded'
def main():
pub_plus = rospy.Publisher('~plus_rect_event', MouseEvent, queue_size=1)
pub_minus = rospy.Publisher('~minus_rect_event', MouseEvent, queue_size=1)
rospy.loginfo('Waiting for image_view2 launch..')
rospy.sleep(5)
width = rospy.get_param('~image_width')
height = rospy.get_param('~image_height')
plus_events = [
MouseEvent(type=3, x=width/4, y=height/4, width=width, height=height),
MouseEvent(type=4, x=width/2, y=height/2, width=width, height=height),
MouseEvent(type=2, x=3*width/4, y=3*height/4, width=width, height=height),
]
minus_events = [
MouseEvent(type=3, x=3*width/4, y=3*height/4, width=width, height=height),
MouseEvent(type=4, x=width/2, y=height/2, width=width, height=height),
MouseEvent(type=2, x=width/4, y=height/4, width=width, height=height),
]
rate = rospy.Rate(10)
for e in plus_events:
e.header.stamp = rospy.get_rostime()
pub_plus.publish(e)
rate.sleep()
for e in minus_events:
e.header.stamp = rospy.get_rostime()
pub_minus.publish(e)
rate.sleep()
def run(self):
lastCommandSent = None
while self.checkRunning():
cmd = None
if lastCommandSent is None or rospy.get_rostime().to_sec()-lastCommandSent > 0.2:
cmd = self.getCommand()
if cmd is not None:
self.ser.write(cmd)
self.ser.flush()
lastCommandSent = rospy.get_rostime().to_sec()
data = ''
t = ''
while t != '\n' and self.checkRunning():
if self.ser.inWaiting() > 0:
t = self.ser.read(1)
if (t != '\r' and t != '\n'):
data += t
elif len(data) == 0:
break
else:
time.sleep(0.01)
if len(data) > 0 and t == '\n' and self.checkRunning():
self.addData(data)
time.sleep(0.001)
def wait_for_data(self, duration):
""" Waits to receive statistics data """
start_time = rospy.get_rostime()
while not rospy.is_shutdown() and not (rospy.get_rostime() > (start_time + rospy.Duration(duration))):
if len(self.graph.nodes) >= len(EXPECTED_NODES) and len(self.graph.topics) >= len(EXPECTED_TOPICS):
return
rospy.sleep(1.0)
def process_pose(self, pose_msg): #Get the pose from the message x = pose_msg.pose.pose.position.x y = pose_msg.pose.pose.position.y z = pose_msg.pose.pose.position.z x_dot = pose_msg.twist.twist.linear.x y_dot = pose_msg.twist.twist.linear.y r,p,ya = convert_quat_to_euler(pose_msg.pose.pose.orientation) vel_wf = np.array([x_dot, y_dot]) rot_mat = np.array([[np.cos(ya), np.sin(ya)], [-np.sin(ya), np.cos(ya)]]) vel_bf = np.dot(rot_mat, vel_wf) v_x = vel_bf[0] v_y = vel_bf[1] #Process the pose to get statistics total_v = (v_x**2 + v_y**2)**.5 if (total_v > self.max_speed): self.max_speed = total_v slip = 0 if (v_x > 0.1): slip = -np.arctan(v_y/np.abs(v_x)) if (slip > self.max_slip): self.max_slip = slip line_eval = (y > self.line[0]*x + self.line[1]) #Check if we've completed the last if ((self.last_eval is not 0) and (line_eval is not self.last_eval) and (x > self.line[2]) and (x < self.line[3])): if (self.start_time is None): self.start_time = rospy.get_rostime() else: self.end_time = rospy.get_rostime() self.lap_time = self.end_time.to_sec() - self.start_time.to_sec() #Read the launch params self.params = get_launch_params(self.prefix) self.publish_msg() self.reset_lap() self.last_eval = line_eval
def run(self):
#every 2 seconds
rate = rospy.Rate(0.5)
now = rospy.get_rostime()
while self.active and not rospy.is_shutdown():
lastEpoch = now
rate.sleep()
now = rospy.get_rostime()
self.connectedLock.acquire(shared=True)
#get a snapshot of the connectedClients
connectedSnapshot = self.getConnectedClients()
#calculate who the leader is
newLeader = self.calculateLeader(now, connectedSnapshot)
self.connectedLock.release()
if self.leader:
rospy.loginfo("aggregator leader: %s - %s", self.leader.topic, self.weights)
if not newLeader == self.leader and newLeader is not None:
self.leader = newLeader
self.swapForwarding()
leaderMsgs = self.leader.msgsFromEpoch(now)
if leaderMsgs:
self.forwardingCallback(leaderMsgs[-1][1])
def Control12(self):
t= 0.0
dt= self.time_step
#base= self.joint_positions[1]
base= [0.0]*7
goal= pr2_controllers_msgs.msg.JointTrajectoryGoal()
goal.trajectory.header.stamp = rospy.Time.now()
goal.trajectory.joint_names= self.joint_names[1];
goal.trajectory.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
if self.over_target:
goal.trajectory.points.append(trajectory_msgs.msg.JointTrajectoryPoint())
while t<3.14159*4.0:
t= t+dt
goal.trajectory.points[0].time_from_start= rospy.Duration(dt)
goal.trajectory.points[0].positions= base + np.array([self.Curve(t)]*7)
#goal.trajectory.points[0].velocities= [0.0]*7
goal.trajectory.points[0].velocities= [self.CurveD(t)]*7
if self.over_target:
goal.trajectory.points[1].time_from_start= rospy.Duration(dt*2)
goal.trajectory.points[1].positions= base + np.array([self.Curve(t+dt)]*7)
#goal.trajectory.points[1].velocities= [0.0]*7
goal.trajectory.points[1].velocities= [self.CurveD(t+dt)]*7
print goal.trajectory.points
goal.trajectory.header.stamp= rospy.Time.now()
self.traj_client.send_goal(goal)
#self.traj_client.wait_for_result()
start_time= rospy.get_rostime()
while rospy.get_rostime() < start_time + rospy.Duration(dt):
time.sleep(dt*0.1)
def computeOrientation(start, goal):
global tolerance
rospy.wait_for_service('move_base_node/NavfnROS/make_plan')
try:
# Initiating the connection to the make plan service from the move base node.
make_plan_connection = rospy.ServiceProxy('move_base_node/NavfnROS/make_plan', GetPlan)
start = PoseStamped()
goal = PoseStamped()
start.header.stamp = rospy.get_rostime()
goal.header.stamp = rospy.get_rostime()
start.header.frame_id = '/map'
goal.header.frame_id = '/map'
start.pose = startPose
goal.pose = goalPose
# Calculation of the path by move base
response = make_plan_connection(start,goal,tolerance)
deltaX = start.pose.pose.position.x - path[0].pose.pose.position.x
deltaY = start.pose.pose.position.y - path[0].pose.pose.position.y
yaw = atan2(deltaY,deltaX)
return yaw
except rospy.ServiceException, e:
print "Service call failed: %s" %e
def update_encoders(arg):
#Fix indentation
encoderr.update(GPIO.input("P9_23"),GPIO.input("P9_24"),rospy.get_rostime().to_sec())
left = JointState()
#Add joint name and timestamps
print rospy.get_rostime().to_sec()
left.header.frame_id = "front_left_wheel_link"
left.header.stamp = arg.current_real
left.position.append(encoderr.position)
left.velocity.append(encoderr.velocity)
encoderl.update(GPIO.input("P8_17"),GPIO.input("P8_26"),rospy.get_rostime().to_sec())
right = JointState()
right.header.frame_id = "front_right_wheel_link"
right.header.stamp = arg.current_real
right.position.append(encoderl.position)
right.velocity.append(encoderl.velocity)
rospy.loginfo(left)
publ.publish(left)
rospy.loginfo(right)
pubr.publish(right)
rearleft = JointState()
rearleft.header.frame_id = "rear_left_wheel_link"
rearleft.header.stamp = arg.current_real
rearleft.position.append(encoderr.position)
rearleft.velocity.append(encoderr.velocity)
rospy.loginfo(rearleft)
rearright = JointState()
rearright.header.frame_id = "rear_right_wheel_link"
rearright.header.stamp = arg.current_real
rearright.position.append(encoderl.position)
rearright.velocity.append(encoderl.velocity)
def test_tf():
rospy.init_node('tf_test', anonymous=True)
rate = rospy.Rate(100)
init_time = rospy.get_rostime().to_sec()
motor_publisher = rospy.Publisher('/motor_controller/command', Float64)
broadcaster = tf.TransformBroadcaster()
a = 0
while not rospy.is_shutdown():
t = rospy.get_rostime().to_sec() - init_time
if a >= 6.28:
a = a - 0.5
else:
a = a + 0.5
radius = 0.5
x = np.cos(a) * radius
y = np.sin(a) * radius
motor_publisher.publish(a)
broadcaster.sendTransform((x, y, 0), tf.transformations.quaternion_from_euler(-3.14/2, 0, a + 3.14), rospy.Time.now(),
"laser1", "world")
broadcaster.sendTransform((0, 0, radius), tf.transformations.quaternion_from_euler(0, 3.14/2, a), rospy.Time.now(),
"laser2", "world")
print('time: ' + str(t))
print('angle: ' + str(a))
rate.sleep()
def push_to_path(self, PoseStamped):
print 'pushing to path'
self.path.header.seq = 1
self.path.header.frame_id = PoseStamped.header.frame_id
self.path.header.stamp = rospy.get_rostime()
self.path.poses.append(deepcopy(PoseStamped)) # Deepcopy avoids pointing to the same element for all the path
self.path.header.stamp = rospy.get_rostime()
#! /usr/bin/env python
import roslib
roslib.load_manifest('irp6_launch')
import rospy
import actionlib
from trajectory_msgs.msg import *
from pr2_controllers_msgs.msg import *
if __name__ == '__main__':
rospy.init_node('simple_trajectory')
client = actionlib.SimpleActionClient(
'irp6p_controller/joint_trajectory_action', JointTrajectoryAction)
client.wait_for_server()
goal = JointTrajectoryGoal()
goal.trajectory.joint_names = [
'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'
]
goal.trajectory.points.append(
JointTrajectoryPoint([0.0, -1.57, 0.0, 0.0, 4.71, -2.9],
[0, 0, 0, 0, 0, 0], [], rospy.Duration(10.0)))
goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.1)
client.send_goal(goal)
client.wait_for_result()
#!/usr/bin/python
PKG = 'demo'
import roslib
roslib.load_manifest(PKG)
import rospy, sys, os
#remove or add the message type
from nav_msgs.msg import Odometry
rospy.init_node('stamp_publisher')
pub=rospy.Publisher('/stamp', Odometry, queue_size=1, latch=False)
if len(sys.argv) < 2:
print("usage: stamp_publisher.py rate")
exit(1)
rate = float(sys.argv[1])
rospy.loginfo("rate : %f", rate)
#set a publishing rate. Below is a publishing rate of 10 times/second
r=rospy.Rate(rate)
msg = Odometry()
while not rospy.is_shutdown():
msg.header.stamp = rospy.get_rostime()
pub.publish(msg)
r.sleep()
#Get data and publish infinitly
for line in logFile:
msgNum = msgNum + 1
data_t = line.split(',')
d_dist = int(data_t[0].strip()) / 1000.0
d_angle = int(data_t[1].strip()) / 1000.0
range_raw = int(data_t[2].strip())
# compute odometry
robot_theta_i = robot_theta_i + 2 * d_angle / robot_description_r
robot_x_i = robot_x_i + d_dist * math.cos(robot_theta_i)
robot_y_i = robot_y_i + d_dist * math.sin(robot_theta_i)
odoMsg.header.stamp = rospy.get_rostime()
odoMsg.pose.pose.position.x = robot_x_i
odoMsg.pose.pose.position.y = robot_y_i
odoMsg.pose.pose.position.z = 0.0
odoMsg.pose.pose.orientation.x = 0.0
odoMsg.pose.pose.orientation.y = 0.0
odoMsg.pose.pose.orientation.z = math.sin(robot_theta_i / 2.0)
odoMsg.pose.pose.orientation.w = math.cos(robot_theta_i / 2.0)
odomPub.publish(odoMsg)
scanMsg.header.stamp = rospy.get_rostime()
scanRange = getRangeFromRawData(range_raw)
scanMsg.ranges = [scanRange, scanRange]
scanPub.publish(scanMsg)
print("publishing msg #{0}".format(msgNum))
time.sleep(1.0)
depth_path = path_name + "/depth_0/%06d.npy" % image_index
if (not os.path.isfile(left_img_path)) or (
not os.path.isfile(right_img_path)):
break
left_img = cv2.imread(left_img_path, 0)
right_img = cv2.imread(right_img_path, 0)
depth_np = np.load(depth_path)
depth_np = 386.1448 / depth_np #00-02
# depth_np = 379.8145 / depth_np #04-12
depth_visual = np.clip(depth_np, 0.5, 60)
depth_visual = depth_visual / 60.0 * 255.0
depth_visual = depth_visual.astype(np.uint8)
col = cv2.applyColorMap(depth_visual, cv2.COLORMAP_RAINBOW)
pub_ros_time = rospy.get_rostime()
left_msg = cv_bridge.cv2_to_imgmsg(left_img, "mono8")
left_msg.header.stamp = pub_ros_time
left_pub.publish(left_msg)
right_msg = cv_bridge.cv2_to_imgmsg(right_img, "mono8")
right_msg.header.stamp = pub_ros_time
right_pub.publish(right_msg)
depth_msg = cv_bridge.cv2_to_imgmsg(depth_np, "32FC1")
depth_msg.header.stamp = pub_ros_time
depth_pub.publish(depth_msg)
depth_visual_msg = cv_bridge.cv2_to_imgmsg(col, "bgr8")
depth_visual_msg.header.stamp = pub_ros_time
depth_visual_pub.publish(depth_visual_msg)
def run():
"""
Based on the received command, land,takeoff, go to a waypoint, pivot and go to waypoint or go to origin
"""
global currentDroneData , actionState, targetInMap, latchStartTime, latched, wayHomePtr, pub_cmd_vel, pub_takeoff, pub_land, pub_reset
latchTime = rospy.Duration(5.0)
rospy.loginfo("Waiting for a command")
# Loop until ros is shutdown
while not rospy.is_shutdown():
# Keep publish waypoints to ros
publishWaypoints()
# Keep publish ardrone position to ros
publishArdronePos()
# Reset the latch time
if actionState == SYS_DEFS.RESET_LATCH_TIME_ACTION_STATE:
latched = False
# Take off
elif actionState == SYS_DEFS.TAKE_OFF_ACTION_STATE:
# Check if the drone is not latched
if not latched:
latchStartTime = rospy.get_rostime()
latched = True
# Check if ros time is less than start time + latch time
if rospy.get_rostime() < latchStartTime + latchTime:
pub_takeoff.publish(takeoff_msg)
rospy.loginfo("Taking off")
else:
# Hover and reset action state
command(0, 0, 0, 0, 0, 0)
actionState = 0
# Land
elif actionState == SYS_DEFS.LAND_ACTION_STATE:
# Decrease the altitude of the drone, until it reaches 0.5
if currentDroneData.z <= 0.5:
# Check if the drone is not latched
if not latched:
# assign ros time
latchStartTime = rospy.get_rostime()
latched = True
# Check if ros time is less than start time + latch time
if rospy.get_rostime() < latchStartTime + latchTime:
# Land the drone
pub_land.publish(land_msg)
rospy.loginfo("Landing...")
else:
actionState = 0
else:
# Decrease the altitude
command(0, 0, -1, 0, 0, 0)
# Go trough the landing state again
actionState = 2
# Reset
elif actionState == SYS_DEFS.RESET_DRONE_ACTION_STATE:
rospy.loginfo("Resetting")
# Publish reset message
pub_reset.publish(reset_msg)
# Set action state to 0
actionState = 0
# Go to the waypoint without looking
elif actionState == SYS_DEFS.GO_TO_WAYPOINT_WITHOUT_LOOKING_ACTION_STATE:
# Convert drone coordinates into drone frames
returnTargetInDrone(targetInMap)
# Keep going if the waypoint is not reached
if not wayPointReached(SYS_DEFS.WAYPOINT_ACCURACY):
xAct = (targetInDrone.position.x * SYS_DEFS.POINT_GAIN)
yAct = (targetInDrone.position.y * SYS_DEFS.POINT_GAIN)
zAct = (targetInDrone.position.z * SYS_DEFS.POINT_GAIN)
rospy.loginfo("Real Pose X: " + str(realPose.pose.position.x) +
" Y: " + str(realPose.pose.position.y) +
" Z: " + str(realPose.pose.position.z))
rospy.loginfo("Acceleration X: " + str(xAct) +
" Y: " + str(yAct) +
" Z: " + str(zAct))
rospy.loginfo("Current DD X: " + str(currentDroneData.x) +
" Y: " + str(currentDroneData.y) +
" Z: " + str(currentDroneData.z))
rospy.loginfo("Target DD X: " + str(targetInDrone.position.x) +
" Y: " + str(targetInDrone.position.y) +
" Z: " + str(targetInDrone.position.z))
command(xAct, yAct, zAct, 0, 0, 0)
else:
rospy.loginfo("Waypoint Reached ")
command(0, 0, 0, 0, 0, 0)
actionState = 0
# Look at the waypoint
elif actionState == SYS_DEFS.LOOK_AT_WAYPOINT_ACTION_STATE:
rospy.loginfo("Looking at the waypoint")
# Convert drone coordinates into drone frames
returnTargetInDrone(targetInMap)
# Adjust z rotation
zRotAct = targetInDrone.orientation.z * SYS_DEFS.POINT_GAIN
command(0, 0, 0, 0, 0, zRotAct)
# Look and Go to the waypoint
elif actionState == SYS_DEFS.LOOK_AND_GO_TO_WAYPOINT_ACTION_STATE:
# Convert drone coordinates into drone frames
returnTargetInDrone(targetInMap)
# Keep going if the waypoint is not reached
if not wayPointReached(SYS_DEFS.WAYPOINT_ACCURACY):
# Check if the drone is facing the waypoint, fix orientation if not
if wayPointFaced(SYS_DEFS.ANGLE_ACCURACY):
# Adjust accelleration with angle gain and point gain
zRotAct = (targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN)
xAct = (targetInDrone.position.x * SYS_DEFS.POINT_GAIN)
yAct = (targetInDrone.position.y * SYS_DEFS.POINT_GAIN)
zAct = (targetInDrone.position.z * SYS_DEFS.POINT_GAIN)
rospy.loginfo("Real Pose X: " + str(realPose.pose.position.x) +
" Y: " + str(realPose.pose.position.y) +
" Z: " + str(realPose.pose.position.z))
rospy.loginfo("Acceleration X: " + str(xAct) +
" Y: " + str(yAct) +
" Z: " + str(zAct))
rospy.loginfo("Current DD X: " + str(currentDroneData.x) +
" Y: " + str(currentDroneData.y) +
" Z: " + str(currentDroneData.z))
rospy.loginfo("Target DD X: " + str(targetInDrone.position.x) +
" Y: " + str(targetInDrone.position.y) +
" Z: " + str(targetInDrone.position.z))
command(xAct, yAct, zAct, 0, 0, zRotAct)
else:
# Fix orientation if the drone is not facing the waypoint
rospy.loginfo("Fixing orientation")
zRotAct = targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN
command(0, 0, 0, 0, 0, zRotAct)
else:
# Waypoint is reached
rospy.loginfo("Waypoint reached ")
# Now hover
command(0, 0, 0, 0, 0, 0)
# Reset to state 0
actionState = 0
# Follow Flightpath
elif actionState == SYS_DEFS.FOLLOW_FLIGHT_PATH_WAYPOINTS_ACTION_STATE:
# Get the global counter
global currentWaypointCounterForFlightPath
# Get a coordinate from the xml file and assign it to targetInMap
targetInMap = droneWaypointsFromXML.getWaypointsCoordinates()
# Check if the anchor is reached
if droneWaypointsFromXML.extractCoordinatesFromXML(currentWaypointCounterForFlightPath):
# Convert drone coordinates into drone frames
returnTargetInDrone(targetInMap)
# Keep going if the waypoint is not reached
if not wayPointReached(SYS_DEFS.WAYPOINT_ACCURACY):
# Check if the drone is facing the waypoint, fix orientation if not
if wayPointFaced(SYS_DEFS.ANGLE_ACCURACY):
# Adjust accelleration with angle gain and point gain
zRotAct = (targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN)
xAct = (targetInDrone.position.x * SYS_DEFS.POINT_GAIN)
yAct = (targetInDrone.position.y * SYS_DEFS.POINT_GAIN)
zAct = (targetInDrone.position.z * SYS_DEFS.POINT_GAIN)
command(xAct, yAct, zAct, 0, 0, zRotAct)
else:
# Fix orientation if the drone is not facing the waypoint
rospy.loginfo("Fixing orientation")
zRotAct = targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN
command(0, 0, 0, 0, 0, zRotAct)
else:
# Waypoint is reached, move to the next one
rospy.loginfo("Target DD X: " + str(targetInMap.position.x) +
" Y: " + str(targetInMap.position.y) +
" Z: " + str(targetInMap.position.z))#
# Increase counter, so we can move to the next waypoint
currentWaypointCounterForFlightPath += 1
rospy.loginfo("Waypoint Reached " + str(currentWaypointCounterForFlightPath))
else:
# There are no more waypoints to go through
rospy.loginfo("XML waypoints finished")
# Reset counter to 0
currentWaypointCounterForFlightPath = 0
# Now hover so user can see that there are no more anchors
command(0, 0, 0, 0, 0, 0)
# Reset to actionState 0
actionState = 0
# Follow Flightpath for DWM1001
elif actionState == SYS_DEFS.FOLLOW_FLIGHT_PATH_DWM1001_ACTION_STATE:
# Get the global counter
global currentWaypointCounterForFlightPathDWM1001
# Get a coordinate from anchors and assign it to targetInMap
targetInMap = gazeboDwm1001.getAnchorCoordinates()
# Check if the anchor is reached
if gazeboDwm1001.anchorsReached(currentWaypointCounterForFlightPathDWM1001):
# Convert drone coordinates into drone frames
returnTargetInDrone(targetInMap)
# Keep going if the waypoint is not reached
if not wayPointReached(SYS_DEFS.WAYPOINT_ACCURACY):
# Check if the drone is facing the waypoint, fix orientation if not
if wayPointFaced(SYS_DEFS.ANGLE_ACCURACY):
# Adjust accelleration with angle gain and point gain
zRotAct = (targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN)
xAct = (targetInDrone.position.x * SYS_DEFS.POINT_GAIN)
yAct = (targetInDrone.position.y * SYS_DEFS.POINT_GAIN)
zAct = (targetInDrone.position.z * SYS_DEFS.POINT_GAIN)
command(xAct, yAct, zAct, 0, 0, zRotAct)
else:
# Fix orientation if the drone is not facing the waypoint
rospy.loginfo("Fixing orientation")
zRotAct = targetInDrone.orientation.z * SYS_DEFS.ANGLE_GAIN
command(0, 0, 0, 0, 0, zRotAct)
else:
# Anchor is reached, move to the second one
rospy.loginfo("Target DD X: " + str(targetInMap.position.x) +
" Y: " + str(targetInMap.position.y) +
" Z: " + str(targetInMap.position.z))
# Increase counter, so we can move to the next anchor
currentWaypointCounterForFlightPathDWM1001 += 1
rospy.loginfo("Anchor Reached " + str(currentWaypointCounterForFlightPathDWM1001))
else:
# There are no more anchors to go through
rospy.loginfo("Anchors finished")
# Reset counter to 0
currentWaypointCounterForFlightPathDWM1001 = 0
# Now hover so user can see that there are no more anchors
command(0, 0, 0, 0, 0, 0)
# Reset to actionState 0
actionState = 0
# Publish message twist produced by action state
pub_cmd_vel.publish(messageTwist)
rate.sleep()
def send_requests(self, device_id, requests=[]):
if device_id in self.device_ids and requests:
self.requests_msg.stamp = rospy.get_rostime()
self.requests_msg.device_id = device_id
self.requests_msg.requests = requests
self.requests_pub.publish(self.requests_msg)
def position_control():
rospy.init_node('offb_python_node', anonymous=True)
prev_state = current_state
rate = rospy.Rate(20.0) # MUST be more then 2Hz
# send a few setpoints before starting
for i in range(100):
local_pos_pub.publish(pose)
rate.sleep()
count = 0
# wait for FCU connection
while not current_state.connected:
rate.sleep()
last_request = rospy.get_rostime()
while not rospy.is_shutdown():
count = count + 0.05
now = rospy.get_rostime()
if current_state.mode != "OFFBOARD" and (now - last_request >
rospy.Duration(5.)):
set_mode_client(base_mode=0, custom_mode="OFFBOARD")
last_request = now
else:
if not current_state.armed and (now - last_request >
rospy.Duration(5.)):
arming_client(True)
last_request = now
# older versions of PX4 always return success==True, so better to check Status instead
if prev_state.armed != current_state.armed:
rospy.loginfo("Vehicle armed: %r" % current_state.armed)
if prev_state.mode != current_state.mode:
rospy.loginfo("Current mode: %s" % current_state.mode)
offboard_started_time = rospy.get_rostime()
prev_state = current_state
#quaternion_count = quaternion_from_euler(0, 0, count)
#pose.pose.orientation.x = quaternion_count[0]
#pose.pose.orientation.y = quaternion_count[1]
#pose.pose.orientation.z = quaternion_count[2]
#pose.pose.orientation.w = quaternion_count[3]
# Update timestamp and publish pose
now = rospy.get_rostime()
if (current_state.mode == "OFFBOARD"
and now - offboard_started_time <= rospy.Duration(10.)):
pose.header.stamp = rospy.Time.now()
local_pos_pub.publish(pose)
rate.sleep()
else:
#create and send velocity setpoints
twist = Twist()
xvel = (3.0 - 0.4 * (math.fabs(displacement))) * math.cos(angle)
twist.linear.x = xvel
yvel = 3.0 * math.sin(angle)
twist.linear.y = yvel
if (altitude < targetHeight):
twist.linear.z = 0.2
else:
twist.linear.z = 0
twist.angular.x = 0
twist.angular.y = 0
try:
twist.angular.z = -(0.5 * displacement) * (
xvel * math.cos(angle) / angle -
yvel * math.sin(angle) / angle) - 0.5 * (angle)
except:
twist.angular.z = 0
body_vel_pub.publish(twist)
def __init__(self):
self.my_name = rospy.get_param("~my_name")
self.cuprint = CUPrint("{}/etddf".format(self.my_name))
self.blue_agent_names = rospy.get_param("~blue_team_names")
blue_positions = rospy.get_param("~blue_team_positions")
self.topside_name = rospy.get_param("~topside_name")
assert self.topside_name not in self.blue_agent_names
red_agent_name = rospy.get_param("~red_team_name")
self.update_times = []
self.red_agent_exists = red_agent_name != ""
if self.red_agent_exists:
self.red_agent_name = red_agent_name
self.red_agent_id = len(self.blue_agent_names)
self.use_strapdown = rospy.get_param("~use_strapdown")
self.do_correct_strapdown = rospy.get_param("~correct_strapdown")
self.correct_strapdown_next_seq = False
self.position_process_noise = rospy.get_param(
"~position_process_noise")
self.velocity_process_noise = rospy.get_param(
"~velocity_process_noise")
self.fast_ci = rospy.get_param("~fast_ci")
self.force_modem_pose = rospy.get_param("~force_modem_pose")
self.meas_variances = {}
self.meas_variances["sonar_range"] = rospy.get_param(
"~force_sonar_range_var")
self.meas_variances["sonar_az"] = rospy.get_param(
"~force_sonar_az_var")
self.meas_variances["modem_range"] = rospy.get_param(
"~force_modem_range_var")
self.meas_variances["modem_az"] = rospy.get_param(
"~force_modem_az_var")
known_position_uncertainty = rospy.get_param(
"~known_position_uncertainty")
unknown_position_uncertainty = rospy.get_param(
"~unknown_position_uncertainty")
self.is_deltatier = rospy.get_param("~is_deltatier")
if self.is_deltatier:
self.delta_multipliers = rospy.get_param("~delta_tiers")
self.delta_codebook_table = {
"sonar_range": rospy.get_param("~sonar_range_start_et_delta"),
"sonar_azimuth": rospy.get_param("~sonar_az_start_et_delta")
}
self.buffer_size = rospy.get_param("~buffer_space")
if self.is_deltatier:
rospy.Service('etddf/get_measurement_package',
GetMeasurementPackage, self.get_meas_pkg_callback)
self.kf = KalmanFilter(blue_positions, [], self.red_agent_exists, self.is_deltatier, \
known_posititon_unc=known_position_uncertainty,\
unknown_agent_unc=unknown_position_uncertainty)
self.network_pub = rospy.Publisher("etddf/estimate/network",
NetworkEstimate,
queue_size=10)
self.asset_pub_dict = {}
for asset in self.blue_agent_names:
self.asset_pub_dict[asset] = rospy.Publisher("etddf/estimate/" +
asset,
Odometry,
queue_size=10)
if self.red_agent_exists:
self.asset_pub_dict[self.red_agent_name] = rospy.Publisher(
"etddf/estimate/" + self.red_agent_name,
Odometry,
queue_size=10)
self.last_update_time = rospy.get_rostime()
# Modem & Measurement Packages
rospy.Subscriber("etddf/packages_in",
MeasurementPackage,
self.meas_pkg_callback,
queue_size=1)
# Strapdown configuration
self.update_seq = 0
self.strapdown_correction_period = rospy.get_param(
"~strapdown_correction_period")
strap_topic = "odometry/filtered/odom"
rospy.Subscriber(strap_topic,
Odometry,
self.nav_filter_callback,
queue_size=1)
self.intersection_pub = rospy.Publisher("set_pose",
PoseWithCovarianceStamped,
queue_size=1)
self.cuprint("Waiting for strapdown")
rospy.wait_for_message(strap_topic, Odometry)
self.cuprint("Strapdown found")
# Sonar Subscription
rospy.Subscriber("sonar_processing/target_list/associated",
SonarTargetList, self.sonar_callback)
self.cuprint("Loaded")
return mp
if __name__ == "__main__":
rospy.init_node("etddf_node")
et_node = ETDDF_Node()
debug = False
if not debug:
rospy.spin()
else:
o = Odometry()
o.pose.pose.orientation.w = 1
et_node.use_strapdown = False
rospy.sleep(2)
t = rospy.get_rostime()
et_node.nav_filter_callback(o)
mp = MeasurementPackage()
m = Measurement("modem_range", t, "topside", "guppy", 6, 0, [], 0)
mp.measurements.append(m)
m = Measurement("modem_azimuth", t, "topside", "guppy", 45, 0, [], 0)
mp.measurements.append(m)
mp.src_asset = "topside"
et_node.meas_pkg_callback(mp)
rospy.sleep(1)
et_node.kf._filter_artificial_depth(0.0)
et_node.nav_filter_callback(o)
stl = SonarTargetList()
def callback(data):
timer = Timer()
timer.tic()
now = rospy.get_rostime()
rpn_proposals = data.proposals
scores = data.scores.reshape(rpn_proposals, N_CLASSES)
boxes = data.boxes.reshape(rpn_proposals, N_CLASSES * 4)
sc_orientation = data.sc_orientation.reshape(rpn_proposals,
N_CLASSES * N_BINS)
toSend = ObstacleList()
toSend.header = data.header
object_list = []
for cls_ind in xrange(N_CLASSES - 1):
# if cls_ind == 2:
# continue
cls_ind += 1 # skip background
cls_boxes = boxes[:, 4 * cls_ind:4 * (cls_ind + 1)]
cls_scores = scores[:, cls_ind]
cls_kind = np.empty(scores.shape[0])
cls_kind.fill(cls_ind)
cls_sc_orient = sc_orientation[:, N_BINS * cls_ind:N_BINS * cls_ind +
N_BINS]
#TODO: Didi
# if cls_ind == 1:
# # Vans and Cars are supressed together
# # TODO: This should be generic
# cls_ind += 1
# cls_boxes = np.vstack((cls_boxes, boxes[:, 4*cls_ind:4*(cls_ind + 1)]))
# cls_scores = np.hstack((cls_scores, scores[:, cls_ind]))
# cls_sc_orient = np.vstack((cls_sc_orient, sc_orientation))
# cls_kind_2 = np.empty(scores.shape[0])
# cls_kind_2.fill(cls_ind)
# cls_kind = np.hstack((cls_kind, cls_kind_2))
dets = np.hstack((cls_boxes, cls_scores[:, np.newaxis],
cls_kind[:, np.newaxis])).astype(np.float32)
keep = nms(dets[:, :-1], NMS_THRESH)
dets = dets[keep, :]
orients = cls_sc_orient[keep, :]
for ind, det in enumerate(dets):
obj = Obstacle()
temp_bbox = det[0:4].tolist()
bbox = [
coord + y_offset_list[ix] for ix, coord in enumerate(temp_bbox)
]
obj.kind_name = CLASSES[cls_ind]
obj.kind = cls_ind
obj.bbox.x_offset = bbox[0]
obj.bbox.y_offset = bbox[1]
obj.bbox.height = bbox[3] - bbox[1]
obj.bbox.width = bbox[2] - bbox[0]
obj.score = det[4]
obj.alpha = math.pi * (2 * np.argmax(orients[ind, :]) + 1) / N_BINS
#np.argmax(orients[ind,:])
#obj.yaw = math.pi * (2* np.argmax(orients[ind,:]) + 1)/N_BINS
object_list.append(obj)
toSend.obstacles = object_list
pub_filter.publish(toSend)
timer.toc()
text = "[NMS] Filtered with stamp %.3f at %.3f: took %.3fs" \
% (data.header.stamp.to_sec(),now.to_sec(),timer.total_time)
rospy.loginfo(text)
def callService(self, service_uri, service, service_type, service_args=[]):
'''
Calls the service and return the response.
To call the service the ServiceProxy can't be used, because it uses
environment variables to determine the URI of the running service. In our
case this service can be running using another ROS master. The changes on the
environment variables is not thread safe.
So the source code of the rospy.SerivceProxy (tcpros_service.py) was modified.
:param str service_uri: the URI of the service
:param str service: full service name (with name space)
:param service_type: service class
:type service_type: ServiceDefinition: service class
:param service_args: arguments
:return: the tuple of request and response.
:rtype: (request object, response object)
:raise StartException: on error
:see: rospy.SerivceProxy<http://docs.ros.org/kinetic/api/rospy/html/rospy.impl.tcpros_service.ServiceProxy-class.html>
'''
service = str(service)
rospy.loginfo("Call service %s[%s]: %s, %s", utf8(service), utf8(service_uri), utf8(service_type), utf8(service_args))
from rospy.core import parse_rosrpc_uri, is_shutdown
# from rospy.msg import args_kwds_to_message
from rospy.exceptions import TransportInitError, TransportException
from rospy.impl.tcpros_base import TCPROSTransport, DEFAULT_BUFF_SIZE # ,TCPROSTransportProtocol
from rospy.impl.tcpros_service import TCPROSServiceClient
from rospy.service import ServiceException
request = service_type._request_class()
import genpy
try:
now = rospy.get_rostime()
import std_msgs.msg
keys = {'now': now, 'auto': std_msgs.msg.Header(stamp=now)}
genpy.message.fill_message_args(request, service_args, keys)
except genpy.MessageException as e:
def argsummary(args):
if type(args) in [tuple, list]:
return '\n'.join([' * %s (type %s)' % (a, type(a).__name__) for a in args])
else:
return ' * %s (type %s)' % (args, type(args).__name__)
raise StartException("Incompatible arguments to call service:\n%s\nProvided arguments are:\n%s\n\nService arguments are: [%s]" % (e, argsummary(service_args), genpy.message.get_printable_message_args(request)))
# request = args_kwds_to_message(type._request_class, args, kwds)
protocol = TCPROSServiceClient(service, service_type, headers={})
transport = TCPROSTransport(protocol, service)
# initialize transport
dest_addr, dest_port = parse_rosrpc_uri(service_uri)
# connect to service
transport.buff_size = DEFAULT_BUFF_SIZE
try:
transport.connect(dest_addr, dest_port, service_uri, timeout=5)
except TransportInitError as e:
# can be a connection or md5sum mismatch
raise StartException(''.join(["unable to connect to service: ", utf8(e)]))
transport.send_message(request, 0)
try:
responses = transport.receive_once()
if len(responses) == 0:
raise StartException("service [%s] returned no response" % service)
elif len(responses) > 1:
raise StartException("service [%s] returned multiple responses: %s" % (service, len(responses)))
except TransportException as e:
# convert lower-level exception to exposed type
if is_shutdown():
raise StartException("node shutdown interrupted service call")
else:
raise StartException("transport error completing service call: %s" % (utf8(e)))
except ServiceException as e:
raise StartException("Service error: %s" % (utf8(e)))
finally:
transport.close()
transport = None
return request, responses[0] if len(responses) > 0 else None
def nav_filter_callback(self, odom):
# Update at specified rate
t_now = rospy.get_rostime()
delta_t_ros = t_now - self.last_update_time
if delta_t_ros < rospy.Duration(1):
return
self.kf.propogate(self.position_process_noise,
self.velocity_process_noise)
self.update_times.append(t_now)
# Update orientation
last_orientation_quat = odom.pose.pose.orientation
(r, p, y) = tf.transformations.euler_from_quaternion([last_orientation_quat.x, \
last_orientation_quat.y, last_orientation_quat.z, last_orientation_quat.w])
self.last_orientation_rad = y
orientation_cov = np.array(odom.pose.covariance).reshape(6, 6)
if self.use_strapdown:
# last_orientation_dot = odom.twist.twist.angular
# last_orientation_dot_cov = np.array(odom.twist.covariance).reshape(6,6)
# Turn odom estimate into numpy
# Note the velocities are in the base_link frame --> Transform to odom frame # Assume zero pitch/roll
v_baselink = np.array([[
odom.twist.twist.linear.x, odom.twist.twist.linear.y,
odom.twist.twist.linear.z
]]).T
rot_mat = np.array([ # base_link to odom frame
[np.cos(y), -np.sin(y), 0], [np.sin(y),
np.cos(y), 0], [0, 0, 1]
])
v_odom = rot_mat.dot(v_baselink)
mean = np.array([[odom.pose.pose.position.x, odom.pose.pose.position.y, odom.pose.pose.position.z, \
v_odom[0,0], v_odom[1,0], v_odom[2,0]]]).T
cov_pose = np.array(odom.pose.covariance).reshape(6, 6)
cov_twist = np.array(odom.twist.covariance).reshape(6, 6)
cov = np.zeros((6, 6))
cov[:3, :3] = cov_pose[:3, :3] #+ np.eye(3) * 4 #sim
cov[3:, 3:] = rot_mat.dot(cov_twist[:3, :3]).dot(
rot_mat.T) #+ np.eye(3) * 0.03 #sim
my_id = self.blue_agent_names.index(self.my_name)
x_nav, P_nav = self.kf.intersect_strapdown(mean,
cov,
my_id,
fast_ci=False)
if self.do_correct_strapdown and (
self.update_seq % self.strapdown_correction_period == 0):
if x_nav is not None and P_nav is not None:
self.correct_strapdown(odom.header, x_nav, P_nav,
last_orientation_quat,
orientation_cov)
elif self.correct_strapdown_next_seq:
self.correct_strapdown(odom.header, x_nav, P_nav,
last_orientation_quat, orientation_cov)
self.correct_strapdown_next_seq = False
self.publish_estimates(t_now, last_orientation_quat, orientation_cov)
self.last_update_time = t_now
self.update_seq += 1
q = quaternion_from_euler(roll, pitch, yaw)
imuMsg.orientation.x = q[0]
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
imuMsg.header.stamp = rospy.Time.now()
imuMsg.header.frame_id = 'base_imu_link'
imuMsg.header.seq = seq
seq = seq + 1
pub.publish(imuMsg)
if (diag_pub_time < rospy.get_time()):
diag_pub_time += 1
diag_arr = DiagnosticArray()
diag_arr.header.stamp = rospy.get_rostime()
diag_arr.header.frame_id = '1'
diag_msg = DiagnosticStatus()
diag_msg.name = 'Razor_Imu'
diag_msg.level = DiagnosticStatus.OK
diag_msg.message = 'Received AHRS measurement'
diag_msg.values.append(
KeyValue('roll (deg)', str(roll * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('pitch (deg)', str(pitch * (180.0 / math.pi))))
diag_msg.values.append(
KeyValue('yaw (deg)', str(yaw * (180.0 / math.pi))))
diag_msg.values.append(KeyValue('sequence number', str(seq)))
diag_arr.status.append(diag_msg)
diag_pub.publish(diag_arr)
def process_base_loop(self):
# If the last command was over 1 sec ago, stop the base
if (self._last_cmd_vel_time is None
or (rospy.get_rostime() - self._last_cmd_vel_time).to_sec() >
self._deadman_secs):
self._x_linear_cmd = 0.0
self._z_angular_cmd = 0.0
# ---------------------------------
# Calculate and send motor commands
# ---------------------------------
with self._cmd_vel_lock:
x_linear_cmd = self._x_linear_cmd
z_angular_cmd = self._z_angular_cmd
# Clamp the velocities to the max configured for the base
x_linear_cmd = max(-self._max_x_lin_vel,
min(x_linear_cmd, self._max_x_lin_vel))
z_angular_cmd = max(-self._max_z_ang_vel,
min(z_angular_cmd, self._max_z_ang_vel))
cmd = calc_create_speed_cmd(x_linear_cmd, z_angular_cmd,
self._wheel_dist, self._wheel_radius,
self._wheel_slip_factor,
self._ticks_per_rotation,
self._max_drive_secs, self._max_qpps,
self._max_accel)
self._speed_cmd_pub.publish(cmd)
# -------------------------------
# Calculate and publish Odometry
# -------------------------------
# if self._roboclaw_front_stats is None or self._roboclaw_rear_stats is None:
if self._roboclaw_front_stats is None:
rospy.loginfo(
"Insufficient roboclaw stats received, skipping odometry calculation"
)
return
with self._stats_lock:
# Calculate change in encoder readings
m1_front_enc_diff = self._roboclaw_front_stats.m1_enc_val - self._m1_front_enc_prev
m2_front_enc_diff = self._roboclaw_front_stats.m2_enc_val - self._m2_front_enc_prev
# m1_rear_enc_diff = self._roboclaw_rear_stats.m1_enc_val - self._m1_rear_enc_prev
# m2_rear_enc_diff = self._roboclaw_rear_stats.m2_enc_val - self._m2_rear_enc_prev
self._m1_front_enc_prev = self._roboclaw_front_stats.m1_enc_val
self._m2_front_enc_prev = self._roboclaw_front_stats.m2_enc_val
# self._m1_rear_enc_prev = self._roboclaw_rear_stats.m1_enc_val
# self._m2_rear_enc_prev = self._roboclaw_rear_stats.m2_enc_val
# Since we have a two Roboclaw robot, take the average of the encoder diffs
# from each Roboclaw for each side.
# m1_enc_diff = (m1_front_enc_diff + m1_rear_enc_diff) / 2
# m2_enc_diff = (m2_front_enc_diff + m2_rear_enc_diff) / 2
m1_enc_diff = m1_front_enc_diff
m2_enc_diff = m2_front_enc_diff
# We take the nowtime from the Stats message so it matches the encoder values.
# Otherwise we would get timing variances based on when the loop runs compared to
# when the stats were measured..
# Since we have a two Roboclaw robot, take the latest stats timestamp from either
# Roboclaw.
front_stamp = self._roboclaw_front_stats.header.stamp
# rear_stamp = self._roboclaw_rear_stats.header.stamp
# nowtime = max(front_stamp, rear_stamp)
nowtime = front_stamp
x_linear_v, y_linear_v, z_angular_v = calc_base_frame_velocity_from_encoder_diffs(
m1_enc_diff, m2_enc_diff, self._ticks_per_rotation,
self._wheel_radius, self._wheel_dist, self._wheel_slip_factor,
self._last_odom_time, nowtime)
time_delta_secs = (nowtime - self._last_odom_time).to_sec()
self._last_odom_time = nowtime
odom = calc_odometry_from_base_velocity(
x_linear_v, y_linear_v, z_angular_v, self._world_x, self._world_y,
self._world_theta, time_delta_secs, nowtime, self._base_frame_id,
self._world_frame_id)
self._odom_pub.publish(odom)
# -----------------------------------------
# Calculate and broacast tf transformation
# -----------------------------------------
self._world_x = odom.pose.pose.position.x
self._world_y = odom.pose.pose.position.y
self._world_theta = yaw_from_odom_message(odom)
quat = odom.pose.pose.orientation
self._tf_broadcaster.sendTransform((self._world_x, self._world_y, 0),
(quat.x, quat.y, quat.z, quat.w),
nowtime, self._base_frame_id,
self._world_frame_id)
self._last_odom_time = nowtime
rospy.logdebug("World position: [{}, {}] heading: {}".format(
self._world_x, self._world_y, self._world_theta))
rospy.logdebug("Forward speed: {}, Turn speed: {}".format(
self._x_linear_cmd, self._z_angular_cmd))
def initPathMarkers():
# cannot write in one equation!!!
sourcePoint = Marker()
goalPoint = Marker()
neighbourPoint = Marker()
finalPath = Marker()
sourcePoint.header.frame_id = 'path_planner'
goalPoint.header.frame_id = 'path_planner'
neighbourPoint.header.frame_id = 'path_planner'
finalPath.header.frame_id = 'path_planner'
sourcePoint.header.stamp = rospy.get_rostime()
goalPoint.header.stamp = rospy.get_rostime()
neighbourPoint.header.stamp = rospy.get_rostime()
finalPath.header.stamp = rospy.get_rostime()
sourcePoint.ns = "path_planner"
goalPoint.ns = "path_planner"
neighbourPoint.ns = "path_planner"
finalPath.ns = "path_planner"
sourcePoint.action = 0 # add/modify an object
goalPoint.action = 0
neighbourPoint.action = 0
finalPath.action = 0
sourcePoint.id = 0
goalPoint.id = 1
neighbourPoint.id = 2
finalPath.id = 3
# sourcePoint.text = 'sourcePoint'
# goalPoint.text = 'goalPoint'
# neighbourPoint.text = 'neighbourPoint'
# finalPath.text = 'finalPath'
sourcePoint.type = 2 # Sphere
goalPoint.type = 2
neighbourPoint.type = 8 # Points
finalPath.type = 4 # Line Strip
sourcePoint.pose.orientation.w = 1.0
goalPoint.pose.orientation.w = 1.0
neighbourPoint.pose.orientation.w = 1.0
finalPath.pose.orientation.w = 1.0
sourcePoint.pose.position.x = 0.0
sourcePoint.pose.position.y = 0.0
sourcePoint.pose.position.z = 0.0
goalPoint.pose.position.x = 10.0
goalPoint.pose.position.y = 10.0
goalPoint.pose.position.z = 0.0
neighbourPoint.pose.position.x = 0.0
neighbourPoint.pose.position.y = 0.0
neighbourPoint.pose.position.z = 0.0
sourcePoint.scale.x = sourcePoint.scale.y = sourcePoint.scale.z = 1.0
goalPoint.scale.x = goalPoint.scale.y = goalPoint.scale.z = 1.0
neighbourPoint.scale.x = neighbourPoint.scale.y = neighbourPoint.scale.z = 0.1
finalPath.scale.x = 0.5 # scale.x controls the width of the line segments
sourcePoint.color.g = 1.0
goalPoint.color.r = 1.0
neighbourPoint.color.r = 0.8
neighbourPoint.color.g = 0.4
finalPath.color.r = 0.2
finalPath.color.g = 0.2
finalPath.color.b = 1.0
sourcePoint.color.a = 1.0
goalPoint.color.a = 1.0
neighbourPoint.color.a = 0.5
finalPath.color.a = 1.0
return (sourcePoint, goalPoint, neighbourPoint, finalPath)
def run(self):
msg = WheelsCmdStamped()
msg.header.stamp = rospy.get_rostime()
msg.vel_left = 0.1
msg.vel_right = 0.1
self.pub.publish(msg)
def publishTagPositions(self, serialData):
"""
Publish anchors and tag in topics using Tag and Anchor Object
:param networkDataArray: Array from serial port containing all informations, tag xyz and anchor xyz
:returns: none
"""
# Update the wrong message counts
for idx, er in enumerate(self.error_buffer):
if er in serialData:
self.error_cnt[idx] += 1
# Output error, if request 10 times failure
for idx, cnt in enumerate(self.error_cnt):
if cnt > 10:
# print self.error_buffer[idx]
print("===>>> ERROR: Tag {} is Lost".format(
self.error_buffer[idx][-1]))
# print(self.error_cnt )
self.reset_error()
arrayData = [x for x in serialData.strip().split(' ')]
# Initial data checking
if len(self.measure_matrix) < 4 and self.new_measure_received:
# print([x for x in serialData.strip().split(',')], ' ', len([x for x in serialData.strip().split(',')]) )
data_length = len([x for x in serialData.strip().split(',')])
if (data_length is 17):
self.measure_matrix.append(
[x for x in serialData.strip().split(',')])
else:
self.new_measure_received = False
self.measure_matrix = []
print("Initial Wrong , reinitial !")
# Publish the measurements
elif len(self.measure_matrix) is 4:
pub = rospy.Publisher("/tags/distance", String, queue_size=100)
now = rospy.get_rostime()
str12 = "{} {} : 1 -> 2 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[0][5],
self.measure_matrix[0][4]))
str13 = "{} {} : 1 -> 3 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[0][9],
self.measure_matrix[0][8]))
str14 = "{} {} : 1 -> 4 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[0][13],
self.measure_matrix[0][12]))
str21 = "{} {} : 2 -> 1 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[1][1],
self.measure_matrix[1][0]))
str23 = "{} {} : 2 -> 3 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[1][9],
self.measure_matrix[1][8]))
str24 = "{} {} : 2 -> 4 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[1][13],
self.measure_matrix[1][12]))
str31 = "{} {} : 3 -> 1 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[2][1],
self.measure_matrix[2][0]))
str32 = "{} {} : 3 -> 2 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[2][5],
self.measure_matrix[2][4]))
str34 = "{} {} : 3 -> 4 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[2][13],
self.measure_matrix[2][12]))
str41 = "{} {} : 4 -> 1 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[3][1],
self.measure_matrix[3][0]))
str42 = "{} {} : 4 -> 2 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[3][5],
self.measure_matrix[3][4]))
str43 = "{} {} : 4 -> 3 : ".format(now.secs, now.nsecs) + str(
self.to_distance(self.measure_matrix[3][9],
self.measure_matrix[3][8]))
self.reset_error()
for i in range(4):
for j in range(4):
if i != j:
self.publishers[i][j].publish(
Float64(data=self.to_distance(
self.measure_matrix[i][4 * j + 1],
self.measure_matrix[i][4 * j])))
pub.publish(str12)
pub.publish(str13)
pub.publish(str14)
pub.publish(str21)
pub.publish(str23)
pub.publish(str24)
pub.publish(str31)
pub.publish(str32)
pub.publish(str34)
pub.publish(str41)
pub.publish(str42)
pub.publish(str43)
print("{} : message published ".format(self.msg_counter))
self.msg_counter += 1
# print(self.measure_matrix)
self.measure_matrix = []
self.new_measure_received = False
if 'Sending' in arrayData:
# print("New data received ...")
self.new_measure_received = True
def LineTracing():
global rawImage
global angularVelocity
global prevTime
global edgesImage
global error
global totalFinalX
now = rospy.get_rostime()
nowTime = now.nsecs
errorTime = 0
if nowTime < prevTime:
errorTime = nowTime - prevTime + 1000000000
else:
errorTime = nowTime - prevTime
errorTime = errorTime / 100000000.0
prevTime = nowTime
img_hsv = cv2.cvtColor(rawImage, cv2.COLOR_BGR2HSV)
lower_color = (12, 87, 0)
upper_color = (44, 255, 255)
img_mask = cv2.inRange(img_hsv, lower_color, upper_color)
kernel = np.ones((3, 3), np.uint8)
img_mask = cv2.erode(img_mask, kernel, iterations=2)
img_mask = cv2.dilate(img_mask, kernel, iterations=2)
edgesImage = cv2.Canny(img_mask, 50, 150, apertureSize=3)
lines = cv2.HoughLinesP(edgesImage,
0.8,
np.pi / 180,
90,
minLineLength=50,
maxLineGap=100)
if lines is None:
return None
x_array = []
y_array = []
for i in lines:
x1 = i[0][0]
y1 = i[0][1]
x2 = i[0][2]
y2 = i[0][3]
if y1 != y2:
length = math.sqrt(
math.pow(int(x2 - x1), 2) + math.pow(int(y2 - y1), 2))
if length > 100:
x_array.append(i[0][0])
y_array.append(i[0][1])
x_array.append(i[0][2])
y_array.append(i[0][3])
cv2.line(rawImage, (i[0][0], i[0][1]), (i[0][2], i[0][3]),
(0, 0, 255), 2)
if (len(x_array) > 3):
f1 = np.polyfit(y_array, x_array, 1)
final_y = 380
final_x = int(f1[0] * 380 + f1[1])
cv2.circle(rawImage, (final_x, final_y), 5, (255, 255, 0), 8)
error = (320 - final_x) / 1000.0
K_P = 1.1
goal = error * K_P
max_value = 0.45
if goal < -max_value:
goal = -max_value
if goal > max_value:
goal = max_value
# print("angular")
# print(goal)
angularVelocity = goal
import numpy as np
import cv
import tf.transformations as tfm
import rospy
import tf
import sys
def loadSource(filename):
content = cv.Load(filename)
return np.asarray(content)
if __name__ == '__main__':
# Prepare transformation
filename = sys.argv[1]
ymsrc = loadSource(filename)
print("Loaded " + filename)
quaternion = tfm.quaternion_from_matrix(ymsrc)
translation = (ymsrc[0, 3], ymsrc[1, 3], ymsrc[2, 3])
rospy.init_node("velodyne_to_camera_publisher")
sender = tf.TransformBroadcaster(100)
rate = rospy.Rate(25)
while not rospy.is_shutdown():
sender.sendTransform(translation, quaternion, rospy.get_rostime(),
"camera", "velodyne")
print("Sent")
rate.sleep()
def is_separate_trigger(switch):
if rospy.get_rostime().to_sec(
) - switch.last_time_on >= switch.debounce_time:
return True
else:
return False
driving_log = csv.DictWriter(f, fieldnames=["RGB Image","Depth Image", "Steering Angle", "Throttle"])
driving_log.writeheader()
# Keep publishing info until rospy is shut down
print("\n\nStart publishing data via /recorder [steering, throttle] and images is saved at /home/jjs/git/JScar/bag/imgs.\n Press 'X' to stop the recording process.\n")
while not rospy.is_shutdown():
if image is None:
continue
# Convert image to RGB color space
#image = cv2.cvtColor(image, 4)
# image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
# Create image filename for current frame
timestamp = rospy.get_rostime()
# RGB image
fname = str(timestamp)+"_"+str(seq) +"_"+str(round(steering, 2)) + "_"+ str(round(throttle, 2))
fname = RGB_IMG_PATH+fname+".jpg"
# Depth Image
depth_fname = str(timestamp)+"_"+str(seq) +"_"+str(round(steering, 2)) + "_"+ str(round(throttle, 2))
depth_fname = DEPTH_IMG_PATH+depth_fname+".jpg"
# Write images into disk
print(fname)
cv2.imwrite(fname, image)
cv2.imwrite(depth_fname, depth_img)
# new csv line
# driving_log.writerow({'RGB Image': fname, 'Depth Image': depth_fname, 'Steering Angle': steering,'Throttle': throttle,'Speed': speed})
driving_log.writerow({'RGB Image': fname, 'Depth Image': depth_fname, 'Steering Angle': steering,'Throttle': throttle})
# publish data to /car_recorder
def omni_pose_cb(self, msg):
self.publish_rviz_markers()
if self.enabled:
#Get the omni's tip pose in the PR2's torso frame
tip_omni, msg_frame = tfu.posestamped_as_matrix(msg)
self.torso_lift_link_T_omni(tip_omni, msg_frame)
#self.torso_T_omni(tip_omni, msg_frame)
#Publish new arm pose
ps = PoseStamped()
ps.header.frame_id = '/torso_lift_link'
ps.header.stamp = rospy.get_rostime()
ps.pose.position.x = self.tip_tt[0]
ps.pose.position.y = self.tip_tt[1]
ps.pose.position.z = self.tip_tt[2]
ps.pose.orientation.x = self.tip_tq[0]
ps.pose.orientation.y = self.tip_tq[1]
ps.pose.orientation.z = self.tip_tq[2]
ps.pose.orientation.w = self.tip_tq[3]
self.set_goal_pub.publish(ps)
if self.zero_out_forces:
wr = OmniFeedback()
wr.force.x = 0
wr.force.y = 0
wr.force.z = 0
self.omni_fb.publish(wr)
self.zero_out_forces = False
else:
#this is a zero order hold publishing the last received values until the control loop is active again
tip_tt = self.tip_tt
tip_tq = self.tip_tq
ps = PointStamped()
ps.header.frame_id = '/torso_lift_link'
ps.header.stamp = rospy.get_rostime()
ps.point.x = tip_tt[0]
ps.point.y = tip_tt[1]
ps.point.z = tip_tt[2]
ps.pose.orientation.x = tip_tq[0]
ps.pose.orientation.y = tip_tq[1]
ps.pose.orientation.z = tip_tq[2]
ps.pose.orientation.w = tip_tq[3]
self.set_goal_pub.publish(ps)
#this is to make the omni force well move if the arm has moved but the commanded
#position of the arm has not changed
tip_omni, msg_frame = tfu.posestamped_as_matrix(msg)
m_o1 = tfu.transform(self.omni_name, msg_frame,
self.tflistener) * tip_omni
ee_point = np.matrix(tr.translation_from_matrix(m_o1)).T
q = self.robot.get_joint_angles()
pos, rot = self.robot.kinematics.FK(q, self.robot.kinematics.n_jts)
# tip_torso = tfu.transform('/torso_lift_link', self.gripper_tip_frame, self.tflistener) \
# * tfu.tf_as_matrix(([0.,0.,0.], tr.quaternion_from_euler(0,0,0)))
tip_torso = hrl_tr.composeHomogeneousTransform(rot, pos)
#tip_torso = tfu.tf_as_matrix(([pos, rot]))
#def composeHomogeneousTransform(rot, disp):
center_t, center_q = self.omni_T_torso(tip_torso)
center_col_vec = np.matrix(center_t).T
#Send force control info
wr = OmniFeedback()
# offsets (0, -.268, -.15) introduced by Hai in phantom driver
# should be cleaned up at some point so that it is consistent with position returned by phantom -marc
lock_pos = tr.translation_matrix(np.matrix(
[0, -.268, -.150])) * tfu.transform(
self.omni_name + '_sensable', self.omni_name,
self.tflistener) * np.row_stack(
(center_col_vec, np.matrix([1.])))
wr.position.x = (
lock_pos[0, 0]
) * 1000.0 #multiply by 1000 mm/m to get units phantom expects
wr.position.y = (lock_pos[1, 0]) * 1000.0
wr.position.z = (lock_pos[2, 0]) * 1000.0
# wr.position.x = tip_tt[0] #multiply by 1000 mm/m to get units phantom expects
# wr.position.y = tip_tt[1]
# wr.position.z = tip_tt[2]
self.omni_fb.publish(wr)
def __init__(self):
self.LASER_MESSAGE_DEFINED = [
"RAWLASER1", "RAWLASER2", "RAWLASER3", "RAWLASER4"
]
self.OLD_LASER_MESSAGE_DEFINED = [
"FLASER", "RLASER", "LASER3", "LASER4"
]
self.ROBOT_LASER_MESSAGE_DEFINED = ["ROBOTLASER1", "ROBOTLASER2"]
self.GPS_MESSAGE_DEFINED = ["NMEAGGA", "NMEARMC"]
self.ODOM_DEFINED = "ODOM"
self.TRUEPOS_DEFINED = "TRUEPOS"
self.PARAM_DEFINED = "PARAM"
self.SYNC_DEFINED = "SYNC"
self.unknown_entries = []
self.stamp = rospy.get_rostime()
self.pose_msg = Odometry()
self.true_pose_msg = Odometry()
self.laser_msg = LaserScan()
self.tf_odom_robot_msg = TransformStamped()
self.tf_laser_robot_msg = TransformStamped()
self.tf2_msg = TFMessage()
self.tf_tr = tf.Transformer(True, rospy.Duration(2.0))
self.pause = False
self.rate = rospy.get_param("~global_rate", 40)
RAWLASER1_topic = rospy.get_param("~RAWLASER1_topic", "/RAWLASER1")
RAWLASER2_topic = rospy.get_param("~RAWLASER2_topic", "/RAWLASER2")
RAWLASER3_topic = rospy.get_param("~RAWLASER3_topic", "/RAWLASER3")
RAWLASER4_topic = rospy.get_param("~RAWLASER4_topic", "/RAWLASER4")
FLASER_topic = rospy.get_param("~FLASER_topic", "/FLASER")
RLASER_topic = rospy.get_param("~RLASER_topic", "/RLASER")
LASER3_topic = rospy.get_param("~LASER3_topic", "/LASER3")
LASER4_topic = rospy.get_param("~LASER4_topic", "/LASER4")
ROBOTLASER1_topic = rospy.get_param("~ROBOTLASER1_topic",
"/ROBOTLASER1")
ROBOTLASER2_topic = rospy.get_param("~ROBOTLASER2_topic",
"/ROBOTLASER2")
NMEAGGA_topic = rospy.get_param("~NMEAGGA_topic", "/NMEAGGA")
NMEARMC_topic = rospy.get_param("~NMEARMC_topic", "/NMEARMC")
ODOM_topic = rospy.get_param("~ODOM_topic", "/ODOM")
TRUEPOS_topic = rospy.get_param("~TRUEPOS_topic", "/TRUEPOS")
tf_topic = rospy.get_param("~tf_topic", "/tf")
robot_link = rospy.get_param("~robot_link", "base_link")
odom_link = rospy.get_param("~odom_link", "odom")
odom_robot_link = rospy.get_param("~odom_robot_link",
"odom_robot_link")
true_odom_link = rospy.get_param("~trueodom_link", "gt_odom")
ROBOTLASER1_link = rospy.get_param("~ROBOTLASER1_link",
"ROBOTLASER1_link")
ROBOTLASER2_link = rospy.get_param("~ROBOTLASER2_link",
"ROBOTLASER2_link")
# Hacky param to choose which one of ODOM logs or ROBOTLASER1 to publish on tf.
# If set to False, ODOM is pub on tf
# If set to True, ROBOTLASER1 is pub on tf
# Must be careful, if set to True and there are no ROBOTLASER1 but a ODOM
# There won't be anything on tf ...
# TODO : - less hacky way
# - choose which one of ROBOTLASER1 or ROBOTLASER2 etc
self.publish_corrected = rospy.get_param("~pub_corrected", False)
self.topics = {
"RAWLASER1": RAWLASER1_topic,
"RAWLASER2": RAWLASER2_topic,
"RAWLASER3": RAWLASER3_topic,
"RAWLASER4": RAWLASER4_topic,
"FLASER": FLASER_topic,
"RLASER": RLASER_topic,
"LASER3": LASER3_topic,
"LASER4": LASER4_topic,
"ROBOTLASER1": ROBOTLASER1_topic,
"ROBOTLASER2": ROBOTLASER2_topic,
"NMEAGGA": NMEAGGA_topic,
"NMEARMC": NMEARMC_topic,
"ODOM": ODOM_topic,
"TRUEPOS": TRUEPOS_topic,
"TF": tf_topic
}
self.links = {
"ROBOT": robot_link,
"ODOM": odom_link,
"ROBOTODOM": odom_robot_link,
"TRUEPOS": true_odom_link,
"ROBOTLASER1": ROBOTLASER1_link,
"ROBOTLASER2": ROBOTLASER2_link
}
self.params = {
"laser": params.old_laser_params,
"newlaser": params.new_laser_params,
"robot": params.robot_params,
"gps": params.gps_params,
"base": params.base_params,
"arm": params.arm_params,
"segway": params.segway_params
} #TOfinish
def __init__(self):
self.ERRORS = {
0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
0x0001:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
0x0002:
(diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
0x0004:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
0x0008:
(diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Temperature2"),
0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Main batt voltage high"),
0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage high"),
0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
"Logic batt voltage low"),
0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M1 driver fault"),
0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"M2 driver fault"),
0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage high"),
0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Main batt voltage low"),
0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature1"),
0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
"Temperature2"),
0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")
}
rospy.init_node("roboclaw_node")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Connecting to roboclaw")
dev_name = rospy.get_param("~dev", "/dev/ttyACM0")
baud_rate = int(rospy.get_param("~baud", "115200"))
self.address = int(rospy.get_param("~address", "128"))
if self.address > 0x87 or self.address < 0x80:
rospy.logfatal("Address out of range")
rospy.signal_shutdown("Address out of range")
# TODO need someway to check if address is correct
try:
roboclaw.Open(dev_name, baud_rate)
except Exception as e:
rospy.logfatal("Could not connect to Roboclaw")
rospy.logdebug(e)
rospy.signal_shutdown("Could not connect to Roboclaw")
self.updater = diagnostic_updater.Updater()
self.updater.setHardwareID("Roboclaw")
self.updater.add(
diagnostic_updater.FunctionDiagnosticTask("Vitals",
self.check_vitals))
try:
version = roboclaw.ReadVersion(self.address)
except Exception as e:
rospy.logwarn("Problem getting roboclaw version")
rospy.logdebug(e)
pass
if not version[0]:
rospy.logwarn("Could not get version from roboclaw")
else:
rospy.logdebug(repr(version[1]))
roboclaw.SpeedM1M2(self.address, 0, 0)
roboclaw.ResetEncoders(self.address)
self.MAX_SPEED = float(rospy.get_param("~max_speed", "2.0"))
self.TICKS_PER_METER = float(
rospy.get_param("~ticks_per_meter", "4342.2"))
self.BASE_WIDTH = float(rospy.get_param("~base_width", "0.315"))
self.LEFT_MOTOR_NUMBER = int(rospy.get_param("~left_motor_number", 1))
self.encodm = EncoderOdom(self.TICKS_PER_METER, self.BASE_WIDTH)
self.last_set_speed_time = rospy.get_rostime()
self.stopped = True
rospy.Subscriber("cmd_vel", Twist, self.cmd_vel_callback)
rospy.sleep(1)
rospy.logdebug("dev %s", dev_name)
rospy.logdebug("baud %d", baud_rate)
rospy.logdebug("address %d", self.address)
rospy.logdebug("max_speed %f", self.MAX_SPEED)
rospy.logdebug("ticks_per_meter %f", self.TICKS_PER_METER)
rospy.logdebug("base_width %f", self.BASE_WIDTH)
def spin(self):
# state
s = self.sensor_state
odom = Odometry(header=rospy.Header(frame_id=self.odom_frame),
child_frame_id=self.base_frame)
js = JointState(name=[
"left_wheel_joint", "right_wheel_joint", "front_castor_joint",
"back_castor_joint"
],
position=[0, 0, 0, 0],
velocity=[0, 0, 0, 0],
effort=[0, 0, 0, 0])
r = rospy.Rate(self.update_rate)
last_cmd_vel = 0, 0
last_cmd_vel_time = rospy.get_rostime()
last_js_time = rospy.Time(0)
# We set the retry count to 0 initially to make sure that only
# if we received at least one sensor package, we are robust
# agains a few sensor read failures. For some strange reason,
# sensor read failures can occur when switching to full mode
# on the Roomba.
sensor_read_retry_count = 0
while not rospy.is_shutdown():
last_time = s.header.stamp
curr_time = rospy.get_rostime()
# SENSE/COMPUTE STATE
try:
self.sense(s)
transform = self.compute_odom(s, last_time, odom)
# Future-date the joint states so that we don't have
# to publish as frequently.
js.header.stamp = curr_time + rospy.Duration(1)
except select.error:
# packet read can get interrupted, restart loop to
# check for exit conditions
continue
except DriverError:
if sensor_read_retry_count > 0:
rospy.logwarn(
'Failed to read sensor package. %d retries left.' %
sensor_read_retry_count)
sensor_read_retry_count -= 1
continue
else:
raise
sensor_read_retry_count = self._SENSOR_READ_RETRY_COUNT
# Reboot Create if we detect that charging is necessary.
if s.charging_sources_available > 0 and \
s.oi_mode == 1 and \
s.charging_state in [0, 5] and \
s.charge < 0.93*s.capacity:
rospy.loginfo("going into soft-reboot and exiting driver")
self._robot_reboot()
rospy.loginfo("exiting driver")
break
# Reboot Create if we detect that battery is at critical level switch to passive mode.
if s.charging_sources_available > 0 and \
s.oi_mode == 3 and \
s.charging_state in [0, 5] and \
s.charge < 0.15*s.capacity:
rospy.loginfo("going into soft-reboot and exiting driver")
self._robot_reboot()
rospy.loginfo("exiting driver")
break
# PUBLISH STATE
self.sensor_state_pub.publish(s)
self.odom_pub.publish(odom)
if self.publish_tf:
self.publish_odometry_transform(odom)
# 1hz, future-dated joint state
if curr_time > last_js_time + rospy.Duration(1):
self.joint_states_pub.publish(js)
last_js_time = curr_time
self._diagnostics.publish(s, self._gyro)
if self._gyro:
self._gyro.publish(s, last_time)
# ACT
if self.req_cmd_vel is not None:
# check for velocity command and set the robot into full mode if not plugged in
if s.oi_mode != self.operate_mode and s.charging_sources_available != 1:
if self.operate_mode == 2:
self._robot_run_safe()
else:
self._robot_run_full()
# check for bumper contact and limit drive command
req_cmd_vel = self.check_bumpers(s, self.req_cmd_vel)
# Set to None so we know it's a new command
self.req_cmd_vel = None
# reset time for timeout
last_cmd_vel_time = last_time
else:
#zero commands on timeout
if last_time - last_cmd_vel_time > self.cmd_vel_timeout:
last_cmd_vel = 0, 0
# double check bumpers
req_cmd_vel = self.check_bumpers(s, last_cmd_vel)
# send command
self.drive_cmd(*req_cmd_vel)
# record command
last_cmd_vel = req_cmd_vel
r.sleep()
def pose():
msg_grip = JointState()
pub3 = rospy.Publisher('/dvrk/MTML/state_gripper_current',
JointState,
queue_size=1)
msg_grip.header.frame_id = "map"
msg_grip.position = [0]
msg_grip.position[0] = -0.10
im = 0
pub2 = rospy.Publisher('/dvrk/MTML/position_cartesian_current',
PoseStamped,
queue_size=1)
rospy.init_node('poser', anonymous=True)
msg = PoseStamped()
rate = rospy.Rate(10)
msg.header.frame_id = "map"
msg.header.stamp = rospy.get_rostime()
msg.pose.position.x = 0.20
msg.pose.position.y = 0.10
msg.pose.position.z = -0.25
msg.pose.orientation.x = 0.70
msg.pose.orientation.y = 0.70
msg.pose.orientation.z = 0.70
msg.pose.orientation.w = 0
msg_cam = Joy()
while not rospy.is_shutdown():
pub_cam = rospy.Publisher('/dvrk/footpedals/coag', Joy, queue_size=1)
pub = rospy.Publisher('/dvrk/MTML/status', String, queue_size=1)
hello_str = "This"
pub.publish(hello_str)
msg.pose.position.x = msg.pose.position.x
msg.pose.position.y = msg.pose.position.y
msg.pose.position.z = msg.pose.position.z
msg.pose.orientation.x = msg.pose.orientation.x
msg.pose.orientation.y = msg.pose.orientation.y
msg.pose.orientation.z = msg.pose.orientation.z
msg.pose.orientation.w = msg.pose.orientation.w
char = getch()
if (char == "c"):
msg_cam.buttons = [1]
pub_cam.publish(msg_cam)
if (char == "v"):
msg_cam.buttons = [0]
pub_cam.publish(msg_cam)
if (char == "n"):
msg_grip.position[0] = msg_grip.position[0] + 0.1
pub3.publish(msg_grip)
if (char == "m"):
msg_grip.position[0] = msg_grip.position[0] - 0.1
pub3.publish(msg_grip)
if (char == "p"):
print("Stop!")
exit(0)
if char == "a":
msg.pose.position.x = msg.pose.position.x - 0.01
pub2.publish(msg)
rate.sleep()
print("Left pressed")
elif char == "e":
msg.pose.position.y = msg.pose.position.y + 0.01
pub2.publish(msg)
rate.sleep()
print("Right pressed")
elif char == "d":
msg.pose.position.y = msg.pose.position.y - 0.01
pub2.publish(msg)
rate.sleep()
print("Right pressed")
elif char == "f":
msg.pose.position.z = msg.pose.position.z + 0.01
pub2.publish(msg)
rate.sleep()
print("Right pressed")
elif char == "r":
msg.pose.position.z = msg.pose.position.z - 0.01
pub2.publish(msg)
rate.sleep()
print("Right pressed")
elif char == "w":
msg.pose.position.x = msg.pose.position.x + 0.01
pub2.publish(msg)
rate.sleep()
print("Up pressed")
elif char == "l":
getSlavePos()
elif char == "1":
q = [
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
]
q_new = angleUpdate(q, 0, 1)
msg.pose.orientation.x = q_new[0]
msg.pose.orientation.y = q_new[1]
msg.pose.orientation.z = q_new[2]
msg.pose.orientation.w = q_new[3]
pub2.publish(msg)
rate.sleep()
elif char == "2":
q = [
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
]
q_new = angleUpdate(q, 0, -1)
msg.pose.orientation.x = q_new[0]
msg.pose.orientation.y = q_new[1]
msg.pose.orientation.z = q_new[2]
msg.pose.orientation.w = q_new[3]
pub2.publish(msg)
rate.sleep()
elif char == "3":
q = [
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
]
q_new = angleUpdate(q, 1, 1)
msg.pose.orientation.x = q_new[0]
msg.pose.orientation.y = q_new[1]
msg.pose.orientation.z = q_new[2]
msg.pose.orientation.w = q_new[3]
pub2.publish(msg)
rate.sleep()
elif char == "4":
q = [
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
]
q_new = angleUpdate(q, 1, -1)
msg.pose.orientation.x = q_new[0]
msg.pose.orientation.y = q_new[1]
msg.pose.orientation.z = q_new[2]
msg.pose.orientation.w = q_new[3]
pub2.publish(msg)
rate.sleep()
elif char == "5":
q = [
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
]
q_new = angleUpdate(q, 2, 1)
msg.pose.orientation.x = q_new[0]
msg.pose.orientation.y = q_new[1]
msg.pose.orientation.z = q_new[2]
msg.pose.orientation.w = q_new[3]
pub2.publish(msg)
rate.sleep()
elif char == "6":
q = [
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
]
q_new = angleUpdate(q, 2, -1)
msg.pose.orientation.x = q_new[0]
msg.pose.orientation.y = q_new[1]
msg.pose.orientation.z = q_new[2]
msg.pose.orientation.w = q_new[3]
pub2.publish(msg)
rate.sleep()
#!/usr/bin/env python
import rospy
import dynamic_reconfigure.client
def callback(config):
rospy.loginfo("Config set to {int_param}, {double_param}, {str_param}, {bool_param}, {size}".format(**config))
if __name__ == "__main__":
rospy.init_node("dynamic_client")
rospy.wait_for_service("dynamic_srv/set_parameters")
client = dynamic_reconfigure.client.Client("dynamic_srv", timeout=10, config_callback=callback)
r = rospy.Rate(1)
x = 0
b = False
while not rospy.is_shutdown():
x = x+1
if x>10:
x = 0
b = not b
client.update_configuration({"int_param":x, "double_param":(1/(x+1)), "str_param":str(rospy.get_rostime()), "bool_param":b, "size": 1})
r.sleep()
def _tf_handle(msg):
_msgs.append(
(msg, rospy.get_rostime(), msg._connection_header['callerid']))
def has_crossed(self):
""" checks if crossing maneuver is over """
return self.mode == Mode.CROSS and \
rospy.get_rostime() - self.cross_start > rospy.Duration.from_sec(self.params.crossing_time)
def publish(self):
self.marker.header.stamp = rospy.get_rostime()
self.pub_send_marker.publish(self.marker)