def cancel_command():
cancelmessage = GoalID()
cancelmessage.stamp.secs = 0
cancelmessage.stamp.nsecs = 0
cancelmessage.id = ''
cancel_cmd.publish(cancelmessage)
print("TestFunction")
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 stop_navigation(self):
if self.navigation_goal_id == '-1': # only cancel nav if currently underway
return
msg = GoalID() # init cancel message
msg.id = self.navigation_goal_id # specify current goal id
self.move_cancel_pub.publish(msg) # publish message
self.navigation_goal_id = '-1' # update current nav goal id
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 publishNextPoint(self):
x_next, y_next = self.convert_node_to_point(self.next_node)
w_next = self.computeNextOrientation(x_next, y_next)
rospy.loginfo(
"[%s] Converted next node to point: (%s,%s), orientation: %s" %
(self.node_name, x_next, y_next, w_next))
goal_id = GoalID()
goal_id.id = str(self.goal_idx)
msg = MoveBaseActionGoal()
msg.goal_id = goal_id
msg.goal.target_pose.header.frame_id = self.frame_id
msg.goal.target_pose.pose.position.x = x_next
msg.goal.target_pose.pose.position.y = y_next
msg.goal.target_pose.pose.position.z = 0
msg.goal.target_pose.pose.orientation.x = 0
msg.goal.target_pose.pose.orientation.y = 0
msg.goal.target_pose.pose.orientation.z = math.sin(w_next / 2)
msg.goal.target_pose.pose.orientation.w = math.cos(w_next / 2)
self.pub_goal.publish(msg)
rospy.loginfo("[%s] Dispatched goal point: (%s,%s)" %
(self.node_name, x_next, y_next))
if self.next_node == self.goal:
self.dispatched_end = True
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 cancel_exploration(self):
if self.exploration_id:
rospy.logerr("Robot {} Cancelling exploration...".format(self.robot_id))
goal_id = GoalID()
goal_id.id = self.exploration_id
self.cancel_explore_pub.publish(goal_id)
else:
rospy.logerr("Exploration ID not set...")
def get_goal_to_publish():
# Get a random goal and go there
# We could try to improve this random search, and have some memory
global latest_goal_time
global latest_goal_id
global goal_points_list
if latest_goal_id == None:
latest_goal_id = 1
latest_goal_time = time.time()
else:
if time.time() - latest_goal_time > 20.:
latest_goal_id += 1
latest_goal_time = time.time()
else:
# Print should not happen anymore
print "Too little time passed since last goal sent"
return None
goal_to_send = MoveBaseActionGoal()
current_goal_id = GoalID()
current_goal_id.id = str(latest_goal_id)
goal_to_send.goal_id = current_goal_id
pose_stamped = PoseStamped()
pose = Pose()
idx = random.randrange(len(goal_points_list))
pose.position.x = goal_points_list[idx][0]
pose.position.y = goal_points_list[idx][1]
roll = 0.
pitch = 0.
yaw = 0.
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
pose_stamped.pose = pose
goal = MoveBaseGoal()
goal.target_pose = pose_stamped
goal.target_pose.header.frame_id = 'map'
goal_to_send.goal = goal
return goal_to_send
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 execute(self, userdata):
rospy.loginfo('Executing Park')
rospy.loginfo('STAYING HERE')
goal_id = GoalID()
goal_id.id = ''
pub = rospy.Publisher('/move_base/cancel', GoalID, queue_size=10)
pub.publish(goal_id)
return 'parkSuccessful'
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 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 cancel_exploration(self):
if self.exploration_id:
pu.log_msg(self.robot_id, "Cancelling exploration...",
self.debug_mode)
goal_id = GoalID()
goal_id.id = self.exploration_id
self.cancel_explore_pub.publish(goal_id)
else:
pu.log_msg(self.robot_id, "Exploration ID not set...",
self.debug_mode)
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 run(self):
pub = rospy.Publisher('/Mock/Dobot_Loader/suctionStatus', GoalID, queue_size=10)
rospy.init_node('AddDetectedObjectServer')
r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
self.listener()
my_message = GoalID()
my_message.id=str(self.current_value)
my_message.stamp.nsecs=int(self.current_value)
my_message.stamp.secs=int(self.current_value)
pub.publish(my_message)
r.sleep()
def cancel_goal(self):
goal_id = GoalID()
goal_id.id = self.last_published_goal.goal_id.id
self.goal_cancel_publisher.publish(goal_id)
self.is_moving = False
# print '\nGoal canceled...'
if self.tour_started:
tts.speak('Tour cancelled')
self.tour_started = False
else:
tts.speak('Goal canceled')
time.sleep(5)
# print '\nReturning to waiting position...'
tts.speak('Returning to waiting position')
time.sleep(2)
self.publish_goal(self.start_location)
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 move_robot_to_goal(self, goal, direction=1):
id_val = "robot_{}_{}_{}".format(self.robot_id, self.goal_count,
direction)
move = MoveToPosition2DActionGoal()
move.header.frame_id = '/robot_{}/map'.format(self.robot_id)
goal_id = GoalID()
goal_id.id = id_val
move.goal_id = goal_id
move.goal.target_pose.x = goal[0]
move.goal.target_pose.y = goal[1]
move.goal.target_distance = self.target_distance
move.goal.target_angle = self.target_angle
self.moveTo_pub.publish(move)
self.goal_count += 1
chosen_point = ChosenPoint()
chosen_point.header.frame_id = '{}'.format(self.robot_id)
chosen_point.x = goal[0]
chosen_point.y = goal[1]
chosen_point.direction = direction
self.chose_point_pub.publish(chosen_point)
self.robot_state = PASSIVE_STATE
self.is_initial_move = False
def publishNextPoint(self):
x_next, y_next = self.convert_node_to_point(self.next_node)
w_next = self.computeNextOrientation(x_next,y_next)
rospy.loginfo("[%s] Converted next node to point: (%s,%s), orientation: %s" %(self.node_name, x_next, y_next, w_next))
goal_id = GoalID()
goal_id.id = str(self.goal_idx)
msg = MoveBaseActionGoal()
msg.goal_id = goal_id
msg.goal.target_pose.header.frame_id = self.frame_id
msg.goal.target_pose.pose.position.x = x_next
msg.goal.target_pose.pose.position.y = y_next
msg.goal.target_pose.pose.position.z = 0
msg.goal.target_pose.pose.orientation.x = 0
msg.goal.target_pose.pose.orientation.y = 0
msg.goal.target_pose.pose.orientation.z = math.sin(w_next/2)
msg.goal.target_pose.pose.orientation.w = math.cos(w_next/2)
self.pub_goal.publish(msg)
rospy.loginfo("[%s] Dispatched goal point: (%s,%s)" %(self.node_name,x_next,y_next))
if self.next_node == self.goal:
self.dispatched_end = True
def move_robot_to_goal(self, goal, direction=1):
id_val = "robot_{}_{}_{}".format(self.robot_id, self.goal_count, direction)
move = MoveToPosition2DActionGoal()
move.header.frame_id = '/robot_{}/map'.format(self.robot_id)
goal_id = GoalID()
goal_id.id = id_val
move.goal_id = goal_id
move.goal.header.frame_id= '/robot_{}/map'.format(self.robot_id)
move.goal.target_pose.x = goal[0]
move.goal.target_pose.y = goal[1]
move.goal.target_distance = self.target_distance
move.goal.target_angle = self.target_angle
self.moveTo_pub.publish(move)
self.goal_count += 1
if self.robot_type == RR_TYPE and direction == BACK_TO_ORIGIN:
self.navigation_start_time = rospy.Time.now()
pu.log_msg(self.robot_id,"New goal ID: {}".format(self.goal_count),self.debug_mode)
chosen_point = ChosenPoint()
chosen_point.header.frame_id = '{}'.format(self.robot_id)
chosen_point.x = goal[0]
chosen_point.y = goal[1]
chosen_point.direction = direction
self.robot_stopped = False
self.chose_point_pub.publish(chosen_point)
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()
def cancel(self):
with self.moving_lock:
if self.is_moving==1:
goal_id=GoalID()
goal_id.id=self.side+"_"+str(self.current_goal_id)
self.pub_traj_cancel.publish(goal_id)
def _on_base_stop_btn_pressed(self):
data8 = GoalID()
data8.id = ""
self._base_stop_pub.publish(data8)
def timer_callback(self, msg):
stop_msg = GoalID()
stop_msg.id = ''
if self.stop:
self.stop_pub.publish(stop_msg)
def chat_server():
server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
server_socket.bind((HOST, PORT))
server_socket.listen(10)
# add server socket object to the list of readable connections
SOCKET_LIST.append(server_socket)
print "Chat server started on port " + str(PORT)
rospy.init_node("publish_string")
pub = rospy.Publisher("publish_string", String, queue_size=10)
stopgoal = rospy.Publisher("move_base/cancel", GoalID, queue_size=10)
data = String()
rate = rospy.Rate(10)
msg = GoalID()
while 1:
# get the list sockets which are ready to be read through select
# 4th arg, time_out = 0 : poll and never block
ready_to_read, ready_to_write, in_error = select.select(
SOCKET_LIST, [], [], 0)
for sock in ready_to_read:
# a new connection request recieved
if sock == server_socket:
sockfd, addr = server_socket.accept()
SOCKET_LIST.append(sockfd)
print "Client (%s, %s) connected" % addr
broadcast(server_socket, sockfd,
"[%s:%s] entered ourchatting room\n" % addr)
# a message from a client, not a new connection
else:
# process data recieved from client,
try:
# receiving data from the socket.
data = sock.recv(RECV_BUFFER)
if data:
# there is something in the socket
broadcast(server_socket, sock, "\r" + data)
myfile = open('testfile2.txt', 'w')
print 'opened file'
myfile.write(str(data))
myfile.close()
print 'finished writing file'
print 'publishing to ROS'
pub.publish(str(data))
print 'Publishing OK'
print data
if str(data) == '0':
msg.id = ''
print "message.id printed"
print msg.id
stopgoal.publish(msg)
print "stopgoal just plublished!"
print 'im here now'
#rospy.spin()
else:
# remove the socket that's broken
if sock in SOCKET_LIST:
SOCKET_LIST.remove(sock)
# at this stage, no data means probably theconnection has been broken
broadcast(server_socket, sock,
"Client (%s, %s)is offline\n" % addr)
# exception
except:
broadcast(server_socket, sock,
"Client (%s, %s) isoffline\n" % addr)
continue
server_socket.close()
