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 callback_main(self, data):
if not self.stop:
if data.x != -1:
self.flag_find = True
if self.flag_explore and self.status_explore_goal == 1 and data.y <= 30:
rospy.loginfo("Stop Explore and kill Operator")
rospy.Publisher("/Explore/cancel", GoalID,
queue_size=1).publish(GoalID())
os.system("rosnode kill /Operator")
time.sleep(5)
self.flag_explore = False
if (self.move_base_info.status_list
and self.move_base_info.status_list[0].status
== 1) and data.y <= 4:
rospy.Publisher('/move_base/cancel', GoalID,
queue_size=1).publish(GoalID())
self.goal_ajustment(data)
elif not self.flag_explore:
self.move_goal_to_object(data.x, data.z)
else:
if not self.flag_find and not self.flag_explore and self.status_explore_goal != 1:
rospy.loginfo("Wait..")
time.sleep(5)
self.pub_explore_goal.publish(ExploreActionGoal())
rospy.loginfo("Start Explore")
self.flag_explore = True
def cancel_command():
cancelmessage = GoalID()
cancelmessage.stamp.secs = 0
cancelmessage.stamp.nsecs = 0
cancelmessage.id = ''
cancel_cmd.publish(cancelmessage)
print("TestFunction")
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 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 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 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 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_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 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 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 odom_callback(msg):
global OLD_MAP, currentX, currentY, currentAngle, last_refreshed, visited, currentRow, currentCol, thinking
# find current (x,y) position of robot based on odometry
currentX = msg.pose.pose.position.x
currentY = msg.pose.pose.position.y
# find current orientation of robot based on odometry (quaternion coordinates)
xOr = msg.pose.pose.orientation.x
yOr = msg.pose.pose.orientation.y
zOr = msg.pose.pose.orientation.z
wOr = msg.pose.pose.orientation.w
# find orientation of robot (Euler coordinates)
(roll, pitch, yaw) = euler_from_quaternion([xOr, yOr, zOr, wOr])
# find currentAngle of robot (equivalent to yaw)
# now that you have yaw, the robot's pose is completely defined by (currentX, currentY, currentAngle)
currentAngle = yaw
if OLD_MAP is None:
return
# cancel the current waypoint and assign a new one if we aren't moving
if not thinking and sitting_still():
backup(400)
clear_pub.publish(Empty())
cancel_pub.publish(GoalID())
currentRow, currentCol = utils.coord_to_row_col((currentX, currentY),
RESOLUTION, MAP)
# mark the area around the region we are in as visited
visited[currentRow - visit_width:currentRow + visit_width,
currentCol - visit_width:currentCol + visit_width] = 1
# check if we are in a region that was unexplored when we
# originally chose this waypoint. If so, the current
# path might take us through a wall, so it is better to
# cancel the waypoint and reroute.
refresh_rate = 10
if (OLD_MAP[currentRow-7:currentRow+8, currentCol-7:currentCol+8] == -1).all() \
and time.time() - last_refreshed > refresh_rate:
print("Reassessing route...")
last_refreshed = time.time()
backup(400)
clear_pub.publish(Empty())
cancel_pub.publish(GoalID())
def on_joy(self, data):
# Set speed ratio using d-pad
twist = Twist()
if data.axes[7] == 1: # moving forward
twist.linear.x = 1
elif data.axes[7] == -1:
twist.linear.x = -1
if data.axes[6] == 1: # rotating left
twist.angular.z = 1
elif data.axes[6] == -1:
twist.angular.z = -1
# Stop
if data.buttons[1]: # B button
twist.linear.x = 0
twist.angular.z = 0
# Publish Twist
self.cmd_vel_pub.publish(twist)
# Cancel move base goal
if data.buttons[2]: # X button
rospy.loginfo('Cancelling move_base goal')
cancel_msg = GoalID()
self.goal_cancel_pub.publish(cancel_msg)
def callback(self, data):
if data.x != -1:
if not self.move_base_info.status_list and self.flag_orientation:
self.orientation_to_obj(data)
elif self.flag_move_to_goal:
self.publisher_move_to_goal(data)
self.flag_move_to_goal = False
self.flag_ajustment = True
self.flag_find = True
elif (self.move_base_info.status_list
and self.move_base_info.status_list[0].status != 1
) and self.flag_ajustment:
self.goal_ajustment(data)
if (self.move_base_info.status_list
and self.move_base_info.status_list[0].status
== 1) and data.y <= 4:
rospy.Publisher('/move_base/cancel', GoalID,
queue_size=1).publish(GoalID())
else:
if self.flag_find and (
self.move_base_info.status_list
and self.move_base_info.status_list[0].status != 1):
self.msg_twist.angular.z = 0.5
self.pub_cmd_vel.publish(self.msg_twist.angular.z)
def goalCallback(self, msg):
self.goal_pose = msg.pose
while True:
# print("robot pose: {}, {}".format(self.robot_pose.position.x, self.robot_pose.position.y))
# print("goal pose: {}, {}".format(self.goal_pose.position.x, self.goal_pose.position.y))
self.distance = sqrt(
pow(self.goal_pose.position.x -
self.robot_pose.position.x, 2) +
pow(self.goal_pose.position.y - self.robot_pose.position.y, 2))
print(self.distance)
if self.first_time == False:
self.end_time = time.time()
if self.distance < self.offset:
if self.first_time == True:
self.start_time = time.time()
self.first_time = False
if self.first_time != None and self.end_time != None:
self.interval_time = abs(self.end_time - self.start_time)
print("TIME START: {}".format(self.start_time))
print("END TIME: {}".format(self.end_time))
print("INTERVAL TIME: {}".format(self.interval_time))
if self.interval_time >= self.stuck_time:
cancel_pub = rospy.Publisher("/move_base/cancel",
GoalID,
queue_size=1)
cancel_msg = GoalID()
cancel_pub.publish(cancel_msg)
rospy.sleep(1)
rospy.loginfo("goal has been canceled")
break
def getGoal(data):
#print("get goal!")
goal.x = data.pose.position.x
goal.y = data.pose.position.y
msg = GoalID()
pubStopMovebase.publish(msg)
receiveGoal = True
def __init__(self):
rospy.loginfo("INIT CONTROL VISION")
rospy.init_node("robot_vision", anonymous=True)
self.control_pid_x = ControlPid(5, -5, 0.01, 0, 0)
self.control_pid_yaw = ControlPid(3, -3, 0.001, 0, 0)
self.flag_find = False
self.flag_explore = False
self.stop = False
self.time_start = time.time()
rospy.Subscriber("/diff/camera_top/camera_info", CameraInfo,
self.callback_camera_info)
rospy.Subscriber("/move_base/status", GoalStatusArray,
self.callback_move_base_info)
rospy.Subscriber("/Explore/status", GoalStatusArray,
self.callback_explore_status)
self.pub_cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
self.pub_move_to_goal = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self.pub_first_map_goal = rospy.Publisher("/GetFirstMap/goal",
GetFirstMapActionGoal,
queue_size=1)
self.pub_explore_goal = rospy.Publisher("/Explore/goal",
ExploreActionGoal,
queue_size=1)
cancel_first_map = rospy.Publisher("/GetFirstMap/cancel",
GoalID,
queue_size=1)
time.sleep(1)
self.pub_first_map_goal.publish()
time.sleep(1)
cancel_first_map.publish(GoalID())
def __init__(self):
"""
JARVIS User Interface Node
"""
rospy.on_shutdown(self.cleanup)
rospy.wait_for_service('return_grips')
self.msg = GoalID()
self.newGrips = jarvis_perception.msg.GraspArray()
self.legalUtterances = [
'upper', 'lower', 'top', 'bottom', 'back', 'front', 'near', 'far',
'rear', 'left', 'right', 'anywhere', 'center'
]
self.lastUtterance = ''
self.axisAlignedBox = ''
self.person_trans = ''
self.person_rot = ''
plotting = True
self.likelihood = None
self.var = None
try:
grip_server = rospy.ServiceProxy('return_grips', ReturnGrips)
self.grips = grip_server()
print self.grips
pubgrips = rospy.Publisher('return_grips',
jarvis_perception.msg.GraspArray,
queue_size=10)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
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(self):
# Cancel all current goals
cancel_pub = rospy.Publisher(rospy.get_name() + '/cancel', GoalID)
cancel_pub.publish(GoalID(stamp=rospy.Time.now()))
# Wait a bit
rospy.sleep(0.5)
def shutdown(self):
try:
for x in self.leds.pins:
wiringpi2.digitalWrite(x, 0)
self.cancel_pub.publish(GoalID())
except:
pass
def __init__(self, node_name, topic_1 = None, msg_type_1 = None, send_topic_1 = None, topic_2 = None, msg_type_2 = None, send_topic_2 = None):
self._motive_topic_1=node_name+'/activate'
if not(topic_1 == None) and not(msg_type_1 == None):
self._topic_1 = topic_1
self._msg_type_1 = msg_type_1
self._send_topic_1=send_topic_1
self.send_1 = FunctionUnit.create_send(self, self._send_topic_1, self._msg_type_1)
else:
self._topic_1 = None
self._msg_type_1 = None
if not(topic_2 == None) and not(msg_type_2 == None):
self._topic_2 = topic_2
self._msg_type_2 = msg_type_2
self._send_topic_2=send_topic_2
self.send_2 = FunctionUnit.create_send(self, self._send_topic_2, self._msg_type_2)
else:
self._topic_2 = None
#print 'init'
FunctionUnit.__init__(self, node_name)
self.motivational_shared_var = False #'True' means the behavior set is activated
self._action_mode = False
self._last_goal = GoalID()
def _generate_id(self):
global g_goal_id
id, g_goal_id = g_goal_id, g_goal_id + 1
now = rospy.Time.now()
return GoalID(id="%s-%i-%.3f" %
(rospy.get_caller_id(), id, now.to_sec()),
stamp=now)
def __init__(self, **kwargs):
assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
"Invalid arguments passed to constructor: %r" % kwargs.keys()
from actionlib_msgs.msg import GoalID
self.goal_id = kwargs.get('goal_id', GoalID())
self.status = kwargs.get('status', int())
self.text = kwargs.get('text', str())
def _getPath(self, path):
if self.calculate_path == True:
self.pubStopNav.publish(GoalID())
self.path = path
print("Path=", path)
else:
pass
def __init__(self, name, user):
self.user = user
self.timer, self.delta = 0., time.clock()
self.poses, self.gids = [], []
self.status = 0
self.name = name
self.key = 'moving'
# Crear mensaje PoseStamped
self.pose = PoseStamped()
self.pose.header.stamp = rospy.Time.now()
self.pose.header.frame_id = "map"
self.pose.pose.position.x = 0
self.pose.pose.position.y = 0
self.pose.pose.position.z = 0
self.pose.pose.orientation.x = 0
self.pose.pose.orientation.y = 0
self.pose.pose.orientation.z = 0
self.pose.pose.orientation.w = 1
self.poses.append(self.pose)
self.pub = rospy.Publisher('/%s/move_base_simple/goal' % self.name,
PoseStamped,
queue_size=100)
self.cancel = GoalID()
self.cancel.stamp = rospy.Time.now()
self.cancel.id = '/%s/HOME' % self.name
self.gids.append(self.cancel)
self.pub_cancel = rospy.Publisher('/%s/move_base/cancel' % self.name,
GoalID,
queue_size=100)
self.subs = rospy.Subscriber('/%s/move_base/status' % self.name,
GoalStatusArray, self.callbackStatus)
self.subs_goal = rospy.Subscriber('/%s/move_base/goal' % self.name,
MoveBaseActionGoal, self.newGoalID)
self.subs_img = rospy.Subscriber(
'/%s/camera/rgb/image_raw' % self.name, Image, self.newImage)
def callback(msg):
global stopMessage
global goHome
global wallFollow
global isWallFollow
message = msg.data
(tempStop, tempGoHome, tempWallFollow) = message.split(":")
#print(tempStop, tempGoHome, tempWallFollow)
if int(tempStop) != int(stopMessage):
# Stop wall follow
isWallFollow = False
cmd_pub.publish("stop")
cancel_nav_pub.publish(GoalID())
tempTwist = Twist()
tempTwist.linear.x = 0
tempTwist.linear.y = 0
tempTwist.linear.z = 0
tempTwist.angular.x = 0
tempTwist.angular.y = 0
tempTwist.angular.z = 0
cmd_vel_pub.publish(tempTwist)
print("Sending stop movement",tempStop, stopMessage)
elif (int(tempGoHome) != int(goHome)):
# Go home
beacon_pub.publish("1")
print("Sending navigate home", goHome, tempGoHome)
elif (int(tempWallFollow) != int(wallFollow)) and (isWallFollow == False):
# Start wall follow
isWallFollow = True
print("lSending run wall follow", tempWallFollow, wallFollow)
cmd_pub.publish("start")
wallFollow = tempWallFollow
goHome = tempGoHome
stopMessage = tempStop
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 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 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)
