def cancel_move_base():
print("canceled move base")
#cancel action
pubCancelAction = rospy.Publisher('/move_base/cancel',
GoalID,
queue_size=0)
answerCancel = GoalID()
answerCancel.stamp = rospy.get_rostime()
answerCancel.id = "map"
pubCancelAction.publish(answerCancel)
#cancel movement
pubCancelMove = rospy.Publisher('cmd_vel', Twist, queue_size=10)
twist = Twist()
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.linear.z = 0.0
twist.angular.x = 0.0
twist.angular.y = 0.0
twist.angular.z = 0.0
pubCancelMove.publish(twist)
def on_enter(self, userdata):
self._startTime = Time.now()
self._failed = False
self._reached = False
goal_id = GoalID()
goal_id.id = 'abcd'
goal_id.stamp = Time.now()
action_goal = MoveBaseGoal()
action_goal.target_pose = userdata.waypoint
action_goal.speed = userdata.speed
if action_goal.target_pose.header.frame_id == "":
action_goal.target_pose.header.frame_id = "world"
try:
self._move_client.send_goal(self._action_topic, action_goal)
resp = self.set_tolerance(goal_id, 0.2, 1.55)
except Exception as e:
Logger.logwarn('Failed to send motion request to waypoint (%(x).3f, %(y).3f):\n%(err)s' % {
'err': str(e),
'x': userdata.waypoint.pose.position.x,
'y': userdata.waypoint.pose.position.y
})
self._failed = True
Logger.loginfo('Driving to next waypoint')
def click_cancel_cb(self, msg):
if msg.data:
print "clicked cancel"
for i in range(3):
cancel_msg = GoalID()
cancel_msg.id = self.last_goal_id
cancel_msg.stamp = rospy.Time.now()
self.mb_cancel_pub.publish(cancel_msg)
def new_goal_id(self):
now = rospy.get_rostime()
new_id = "%s_%i_%i_%i" % (rospy.get_name(), self.count, now.secs, now.nsecs)
self.count += 1
goal_id = GoalID()
goal_id.stamp = now
goal_id.id = new_id
return goal_id
def prepare_and_publish_actionlib_msg(self):
'''
Fill and publish action lib message
'''
goal_id_msg = GoalID()
now = rospy.get_rostime()
goal_id_msg.stamp = now
goal_id_msg.id = "ACTIVE"
def new_goal_id(self):
now = rospy.get_rostime()
new_id = "%s_%i_%i_%i" % (rospy.get_name(), self.count, now.secs,
now.nsecs)
self.count += 1
goal_id = GoalID()
goal_id.stamp = now
goal_id.id = new_id
return goal_id
def prepare_actionlib_msg(self):
'''
Fill action lib message
'''
goal_id_msg = GoalID()
now = rospy.get_rostime()
goal_id_msg.stamp = now
goal_id_msg.id = "ACTIVE"
def PrepareToStop(self, data):
if self.object_x and self.object_y and self.moving and np.sqrt(
np.square(self.object_x - data.pose.pose.position.x) +
np.square(self.object_y - data.pose.pose.position.y)) < 0.5:
cancel_msg = GoalID()
cancel_msg.id = ""
cancel_msg.stamp = rospy.Time.now()
self.cancel_publisher.publish(cancel_msg)
self.moving = False
self.object_x = None
self.object_y = None
def stop_movement(self, timeout=5.0):
'''
description: cancels the goal currently being pursued
needs: mir_move_base_safe and mbot_actions move_base_server
input: tdouble timeout, max time to allow to reach the location before returning
output: bool success, whether the cancelation was successful or not
'''
cancel_msg = GoalID()
cancel_msg.stamp = rospy.Time.now()
if self.__goals_status:
for goal in self.__goals_status:
if goal.status == 0 or goal.status == 1:
goal_id = goal.goal_id
cancel_msg.id = goal_id
self.__cancel_pub.publish(cancel_msg)
return True
return False
def generate_ID(self):
"""
* \brief Generates a unique ID
* \return A unique GoalID for this action
"""
id = GoalID()
cur_time = rospy.Time.now()
ss = self.name + "-"
global s_goalcount_lock
global s_goalcount
with s_goalcount_lock:
s_goalcount += 1
ss += str(s_goalcount) + "-"
ss += str(cur_time.secs) + "." + str(cur_time.nsecs)
id.id = ss
id.stamp = cur_time
return id
def generate_ID(self):
"""
* \brief Generates a unique ID
* \return A unique GoalID for this action
"""
id = GoalID();
cur_time = rospy.Time.now();
ss = self.name + "-";
global s_goalcount_lock
global s_goalcount
with s_goalcount_lock:
s_goalcount += 1
ss += str(s_goalcount) + "-";
ss += str(cur_time.secs) + "." + str(cur_time.nsecs);
id.id = ss;
id.stamp = cur_time;
return id;
def cancel_current_cartesian_path_execution(self):
cancel_goal_id = GoalID()
cancel_goal_id.stamp = rospy.Time.now()
cancel_goal_id.id = self.current_goal_id
self.cancel_execution_publisher.publish(cancel_goal_id)
def resolveCMD(self):
buffer_len = self.device.inWaiting()
# if self.active:
# self.ID = 0
# self.content = [0] * 20
if buffer_len >= (pack_len * 3):
print("need to empty buffer")
self.device.flushInput()
elif buffer_len == 0:
# print("time out occur!")
# self.ID = [0]
# self.content[-1] = 1
# cmd_serial = [9] + self.ID + self.content
# cs = checksum(self.header + [9] + self.ID + self.content)
# cmdstr_start = array("B", self.header + cmd_serial + cs).tostring()
# time.sleep(0.2)
# self.device.write(cmdstr_start)
return
# read data from serial port
header1 = ord(self.device.read(1))
if header1 == self.header[0]:
header2 = ord(self.device.read(1))
if header2 == self.header[1]:
data_len = ord(self.device.read(1))
data = [ord(b) for b in self.device.read(data_len)]
cs_read_high = ord(self.device.read(1))
cs_read_low = ord(self.device.read(1))
cs_read = cs_read_high * 256 + cs_read_low
cs_check = checksum(data)
print(data_len, cs_read, cs_check)
if cs_check == cs_read:
print("checksum pass")
else:
print("checksum wrong!")
self.ID = 0
self.content[0:data_len] = data
self.content[7] = 3
cmd_serial = [self.sendSize] + [self.ID] + self.content
cs = checksum([self.ID] + self.content)
cmdstr_start = array("B", self.header + cmd_serial +
[cs]).tostring()
time.sleep(0.2)
self.device.write(cmdstr_start)
return
else:
print("header 2 wrong!")
self.ID = 0
self.content[7] = 2
cmd_serial = [self.sendSize] + [self.ID] + self.content
cs = checksum([self.ID] + self.content)
cmdstr_start = array("B", self.header + cmd_serial +
[cs]).tostring()
time.sleep(0.2)
self.device.write(cmdstr_start)
return
else:
print("header 1 wrong!")
self.ID = 0
self.content[7] = 2
cmd_serial = [self.sendSize] + [self.ID] + self.content
cs = checksum([self.ID] + self.content)
cmdstr_start = array("B",
self.header + cmd_serial + [cs]).tostring()
time.sleep(0.2)
self.device.write(cmdstr_start)
return
# resolve data after get one full packet
self.ID = data[0]
if self.ID == 1:
# TODO velocity command
self.vx = data[1] + data[2] / 100.0
if data[3] > 127:
self.wz = (data[3] - 255) - (data[4] / 100.0)
else:
self.wz = data[3] + data[4] / 100.0
self.dir = data[6]
print("vx = %s, wz=%s, dir=%s" % (self.vx, self.wz, self.dir))
rospy.loginfo("sending velocity commands")
vel_msg = Twist()
vx = (self.dir - 1) * self.vx
wz = self.wz
vel_msg.linear.x = vx
vel_msg.angular.z = wz
t0 = time.time()
while (time.time() - t0) < 0.2:
self.velPub.publish(vel_msg)
time.sleep(0.01)
vel_msg = Twist()
self.velPub.publish(vel_msg)
elif self.ID == 2:
# TODO move goal command, stop current move base action no matter what
self.moveStatus = 0
goal = GoalID()
goal.stamp = rospy.Time.now()
goal.id = ""
self.cancelPub.publish(goal)
if data[1] > 127:
self.posx = (data[1] - 255) - (data[2] / 100.0)
else:
self.posx = data[1] + (data[2] / 100.0)
if data[3] > 127:
self.posy = (data[3] - 255) - (data[4] / 100.0)
else:
self.posy = data[3] + (data[4] / 100.0)
self.posz = (data[5] * 256 + data[6]) / 10000.0
print("goal pose is %s, %s, %s" %
(self.posx, self.posy, self.posz))
print("sending move goal command")
goal_msg = PoseStamped()
goal_msg.header.frame_id = "/map"
goal_msg.header.stamp = rospy.Time.now()
px, py, pz = self.posx, self.posy, self.posz
goal_msg.pose.position.x = px
goal_msg.pose.position.y = py
[qw, qx, qy, qz] = eul2quat([pz, 0, 0])
goal_msg.pose.orientation.w = qw
goal_msg.pose.orientation.x = qx
goal_msg.pose.orientation.y = qy
goal_msg.pose.orientation.z = qz
self.moveGoalPub.publish(goal_msg)
elif self.ID == 3:
# TODO initial pose command, should check if amcl is turned on
if data[1] > 127:
self.posx = (data[1] - 255) - (data[2] / 100.0)
else:
self.posx = data[1] + (data[2] / 100.0)
if data[3] > 127:
self.posy = (data[3] - 255) - (data[4] / 100.0)
else:
self.posy = data[3] + (data[4] / 100.0)
self.posz = (data[5] * 256 + data[6]) / 10000.0
print("initial pose is %s, %s, %s" %
(self.posx, self.posy, self.posz))
init_msg = PoseWithCovarianceStamped()
init_msg.header.stamp = rospy.Time.now()
init_msg.header.frame_id = '/map'
px, py, pz = self.posx, self.posy, self.posz
init_msg.pose.pose.position.x = px
init_msg.pose.pose.position.y = py
[qw, qx, qy, qz] = eul2quat([pz, 0, 0])
init_msg.pose.pose.orientation.w = qw
init_msg.pose.pose.orientation.x = qx
init_msg.pose.pose.orientation.y = qy
init_msg.pose.pose.orientation.z = qz
self.initialPub.publish(init_msg)
elif self.ID == 4:
# TODO gmapping related commands
cmd_srv = data[7]
arg_srv = data[8] * 256 + data[9]
req = cmdRequest()
req.cmd = cmd_srv
if cmd_srv == 1:
rospy.loginfo("start gmapping")
elif cmd_srv == 2:
rospy.loginfo("stop gmapping")
elif cmd_srv == 3:
rospy.loginfo("pause gmapping")
elif cmd_srv == 4:
rospy.loginfo("resume gmapping")
else:
rospy.logwarn("invalid command")
return
response = self.gmapService(req)
self.moveStatus = response.result
self.cmd_srv = cmd_srv
self.arg_srv = arg_srv
elif self.ID == 5:
# TODO map saver related commands, map filename should base on argument value
cmd_srv = data[7]
arg_srv = data[8] * 256 + data[9]
req = cmdRequest()
req.cmd = cmd_srv
req.str = '/home/tongsky/test'
if cmd_srv == 1:
rospy.loginfo("save map")
elif cmd_srv == 2:
rospy.loginfo("stop save map")
else:
rospy.logwarn("invalid command")
return
response = self.saverService(req)
self.moveStatus = response.result
self.cmd_srv = cmd_srv
self.arg_srv = arg_srv
elif self.ID == 6:
# TODO map server related commands, map filename should base on argument value
cmd_srv = data[7]
arg_srv = data[8] * 256 + data[9]
req = cmdRequest()
req.cmd = cmd_srv
req.str = '/home/tongsky/test.yaml'
if cmd_srv == 1:
rospy.loginfo("import map")
else:
rospy.logwarn("invalid command")
return
response = self.serverService(req)
self.moveStatus = response.result
self.cmd_srv = cmd_srv
self.arg_srv = arg_srv
elif self.ID == 7:
# TODO map server related commands, start amcl should check map imported/ gmapping stop?
cmd_srv = data[7]
arg_srv = data[8] * 256 + data[9]
req = cmdRequest()
req.cmd = cmd_srv
if cmd_srv == 1:
rospy.loginfo("start amcl")
elif cmd_srv == 2:
rospy.loginfo("stop amcl")
else:
rospy.logwarn("invalid command")
return
response = self.amclService(req)
self.moveStatus = response.result
self.cmd_srv = cmd_srv
self.arg_srv = arg_srv
self.active = True
def katana_traject_ctrl():
global read_joint_names
rospy.loginfo(str(read_joint_names))
pub = rospy.Publisher('/katana_arm_controller/joint_trajectory_action/goal', jtag, queue_size=10)
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
g = GoalID()
g.stamp = rospy.Time.now()
g.id = 'SimpleKatana trajectory test - ' + str(h.stamp.secs) + ' . ' + str(h.stamp.nsecs)
jtag_msg = jtag()
jtag_msg.header = h
jtag_msg.goal_id = g
# Create points
p0 = jtp()
p0.positions = joint_pos;
joint_pos_original=joint_pos
p0.time_from_start = rospy.Time.now()
p1 = jtp()
p1.positions = [0,2,0,0,0]
p1.time_from_start = rospy.Time.now() + rospy.Duration(5)
p2 = jtp()
p2.positions = joint_pos_original
p2.time_from_start = rospy.Time.now() + rospy.Duration(10)
# Create the Trajectory Goal
traject = jt()
'''
0: katana_motor1_pan_joint
1: katana_motor2_lift_joint
2: katana_motor3_lift_joint
3: katana_motor4_lift_joint
4: katana_motor5_wrist_roll_joint
'''
traject.joint_names = read_joint_names # ['katana_motor1_pan_joint','katana_motor2_lift_joint','katana_motor3_lift_joint','katana_motor4_lift_joint','katana_motor5_wrist_roll_joint']
traject.points = [p0,p1]
goal = jtag_goal()
goal.trajectory=traject
jtag_msg.goal = goal
rate = rospy.Rate(0.05) # 10hz
while not rospy.is_shutdown():
p0 = jtp()
p0.positions = joint_pos[0:5];
joint_pos_original=joint_pos[0:5]
p0.time_from_start = rospy.Time.now()
p1 = jtp()
p1.positions = [0,2,0,0,0]
p1.time_from_start = rospy.Time.now() + rospy.Duration(8)
p2 = jtp()
p2.positions = joint_pos_original
p2.time_from_start = rospy.Time.now() + rospy.Duration(16)
traject.joint_names = read_joint_names[0:5]
traject.points = [p0,p1,p2]
h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
g.stamp = rospy.Time.now()
g.id = 'SimpleKatana trajectory test - ' + str(h.stamp.secs) + ' . ' + str(h.stamp.nsecs)
rospy.loginfo(jtag_msg)
pub.publish(jtag_msg)
rate.sleep()
