def do_security():
global STATUS_LOCK, CAR_UPDATE_FLAG, CAMERA_UPDATE_FLAG
global CAMERA_CURRENT_TIME, CAR_CURRENT_TIME
global GLOBAL_MOVE_PUB, VEL_PUB
global NAV_FLAG_PUB, ENABLE_MOVE_FLAG, BAR_FLAG
time_now = rospy.Time.now()
STATUS_LOCK.acquire()
if CAR_UPDATE_FLAG:
time1_diff = time_now - CAR_CURRENT_TIME
if CAMERA_UPDATE_FLAG:
time2_diff = time_now - CAMERA_CURRENT_TIME
# 视觉丢失超时保险
# 里程计丢失超时保险
if (CAMERA_UPDATE_FLAG and time2_diff.to_sec() > 180 and not BAR_FLAG) or time1_diff.to_sec() > 5.:
# 发布全局禁止flag
global_move_flag = Bool()
global_move_flag.data = False
GLOBAL_MOVE_PUB.publish(global_move_flag)
car_twist = Twist()
VEL_PUB.publish(car_twist)
CAMERA_UPDATE_FLAG = False
CAR_UPDATE_FLAG = False
CAMERA_CURRENT_TIME = rospy.Time.now()
CAR_CURRENT_TIME = rospy.Time.now()
STATUS_LOCK.release()
def talker():
rospy.init_node("frobit_test_set_speed")
deadman_pub = rospy.Publisher("/fmSignals/deadman", Bool)
deadman = Bool()
cmd_vel_left_pub = rospy.Publisher("/fmSignals/cmd_vel_left", TwistStamped)
twist_left = TwistStamped()
cmd_vel_right_pub = rospy.Publisher("/fmSignals/cmd_vel_right", TwistStamped)
twist_right = TwistStamped()
r = rospy.Rate(node_upd_freq) # set talker update frequency
while not rospy.is_shutdown():
deadman.data = 1 # publish deadman topic
deadman_pub.publish(deadman)
twist_left.header.stamp = rospy.Time.now()
twist_left.twist.linear.x = spd_left
# publish cmd_vel topic for left wheel
cmd_vel_left_pub.publish(twist_left)
twist_right.header.stamp = rospy.Time.now()
twist_right.twist.linear.x = spd_right
# publish cmd_vel topic for right wheel
cmd_vel_right_pub.publish(twist_right)
r.sleep()
def callback_manipulator(self, controller):
if controller.killswitch != self.controller.killswitch:
shut_down = Bool()
shut_down.data = self.controller.killswitch == 1
self.callback_auto_shut_down(shut_down)
self.controller = controller
self.send_manipulator = True
def status_updated_callback(self, msg): # GoalStatusArray
intermediate_statuses = [GoalStatus.PENDING, GoalStatus.ACTIVE, GoalStatus.RECALLING, GoalStatus.PREEMPTING]
for goal in msg.status_list:
# status = GoalManager.status_list[goal.status]
# rospy.logerr("Goal %s. %s" % (status, goal.text))
goal_idx = self.get_index_of(goal.goal_id.id)
if goal.status == GoalStatus.SUCCEEDED: # do a little dance to celebrate
rospy.loginfo("Goal %s reached!" % goal.goal_id.id)
last_goal_reached = Bool()
last_goal_reached.data = not self.loop_goals and goal_idx + 1 == len(self.goals)
self.done_pub.publish(last_goal_reached)
self.wait_for_goal()
if goal_idx + 1 == len(self.goals):
if self.loop_goals:
rospy.loginfo("Now looping back to first goal")
self.reset_goal_list()
self.next_goal()
else:
self.next_goal(goal_idx + 1)
elif goal.status in intermediate_statuses: # transitional state, so just wait
pass
elif goal.status == GoalStatus.PREEMPTED:
rospy.logwarn("Goal %s was preempted. %s" % (goal.goal_id.id, goal.text))
else: # the robot's dead
status = GoalManager.status_list[goal.status]
rospy.logerr("Goal %s %s. %s" % (goal.goal_id.id, status, goal.text))
def navToPose(goal):
"""Drive to a goal subscribed to from /move_base_simple/goal"""
#compute angle required to make straight-line move to desired pose
global xPosition
global yPosition
global theta
#capture desired x and y positions
desiredY = goal.pose.position.y
desiredX = goal.pose.position.x
#capture desired angle
quat = goal.pose.orientation
q = [quat.x, quat.y, quat.z, quat.w]
roll, pitch, yaw = euler_from_quaternion(q)
desiredT = yaw * (180.0/math.pi)
#compute distance to target
goalDistance = math.sqrt(math.pow((desiredX - xPosition),2) + math.pow((desiredY-yPosition),2))#Distance formula
#compute initial turn amount
initialTurn = math.atan((desiredY-yPosition)/(desiredX-xPosition))
print "spin!" #turn to calculated angle
rotate(initialTurn)
print "move!" #move in straight line specified distance to new pose
driveStraight(0.25, goalDistance)
rospy.sleep(2)
print "spin!" #spin to final angle
rotate(desiredT)
print "done"
arrival = Bool()
arrival.data = True
arrival_pub.publish(arrival)
def model_stateCB(self, msg):
if not self.active:
return
quaternion_msg = [msg.pose[1].orientation.x, msg.pose[1].orientation.y, msg.pose[1].orientation.z, msg.pose[1].orientation.w]
euler = tf.transformations.euler_from_quaternion(quaternion_msg)
self.robot_pose = (msg.pose[1].position.x, msg.pose[1].position.y, euler[2])
# print self.robot_pose
# print 'robot_pose:', self.robot_pose#, 'waypoints:', self.waypoints
self.destination_pose = self.circleIntersect(self.lookahead_distance)
if self.destination_pose == None:
self.active = False
print "approach destination "
self.gazebo_cmd(0,0)
msg = Bool()
msg.data = True
self.pub_finish.publish(msg)
else:
# print 'destination_pose:', self.destination_pose
# print 'waypoint_index:', self.current_waypoint_index
self.speed = self.default_speed
distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)
# self.steering_angle = np.arctan((2 * self.robot_length * np.sin(angle_to_destination)) / distance_to_destination)
# print "robot_head",euler[2]*180/math.pi,"angle_to_destination", angle_to_destination*180/math.pi
w = (angle_to_destination + math.pi) / (2 * math.pi) - 0.5
self.gazebo_cmd(self.speed,w)
def newSample(self, msg): newFFT = self.stft.ingestSample(msg.data) if newFFT is not None: self.FFT = newFFT # Mask and average data guard1 = np.mean(newFFT[self.G1_mask, :][:, self.channel_mask]) alpha = np.mean(newFFT[self.Al_mask, :][:, self.channel_mask]) guard2 = np.mean(newFFT[self.G2_mask, :][:, self.channel_mask]) detected = self.movavg.step(alpha > (guard1 + guard2)*1.1) > 0.5 if detected and not self.ignore.test(): self.movavg.reset() self.ignore.reset(4) else: detected = False # Publish messages msg = Float32() msg.data = guard1 self.pub_guard1.publish(msg) msg = Float32() msg.data = alpha self.pub_alpha.publish(msg) msg = Float32() msg.data = guard2 self.pub_guard2.publish(msg) msg = Bool() msg.data = detected self.pub_eyes.publish(msg)
def prepare_std_msg(self):
rospy.loginfo("Preparing std_msgs......")
'''
Fill Bool message
'''
bool_msg = Bool()
bool_msg.data = False
'''
Fill Float32 message
'''
float32_msg = Float32()
float32_msg.data = 1.0
'''
Fill Int32 message
Please, do it your self for practice
'''
'''
Fill String message
Please, do it your self for practice
'''
rospy.loginfo("std_msgs preparation done......")
def pub_e( self , enable ):
""" pub enable status """
msg = Bool()
msg.data = enable
self.pub_enable.publish( msg )
def model_stateCB(self, msg):
if not self.active:
return
quaternion_msg = [msg.pose[1].orientation.x, msg.pose[1].orientation.y, msg.pose[1].orientation.z, msg.pose[1].orientation.w]
euler = tf.transformations.euler_from_quaternion(quaternion_msg)
self.robot_pose = (msg.pose[1].position.x, msg.pose[1].position.y, euler[2])
distance_to_destination= self.getDistance(self.robot_pose, self.destination_pose)
angle_to_destination = -self.getAngle(self.robot_pose, self.destination_pose)
if angle_to_destination < 3 * (math.pi/180):
print "correct straight line"
if distance_to_destination<= self.threshold_proximity:
print "ready to grab"
self.gazebo_cmd(0,0)
self.active = False
self.gripper_cmd(0,0.5)
msg = Bool()
msg.data = True
self.pub_grab_object.publish(msg)
else:
self.gazebo_cmd(self.speed,0)
else:
print "correct orientation"
w = (angle_to_destination + math.pi) / (2 * math.pi) - 0.5
self.gazebo_cmd(0,w)
def run(self):
while not rospy.is_shutdown():
if time.time() - self.current_time > self.time:
shutoff = Bool()
shutoff.data = True
self.pub_shutoff.publish(shutoff)
time.sleep(1)
def grab_close_ball(self):
self.switch_to_cam("arm")
mode = Bool()
mode.data = True
self.arm_mode_pub.publish(mode)
req = False
self.grab_request(req)
def test_std_msgs_Bool(self):
from std_msgs.msg import Bool
self.assertEquals(Bool(), Bool())
self._test_ser_deser(Bool(), Bool())
# default value should be False
self.assertEquals(False, Bool().data)
# test various constructor permutations
for v in [True, False]:
self.assertEquals(Bool(v), Bool(v))
self.assertEquals(Bool(v), Bool(data=v))
self.assertEquals(Bool(data=v), Bool(data=v))
self.assertNotEquals(Bool(True), Bool(False))
self._test_ser_deser(Bool(True), Bool())
self._test_ser_deser(Bool(False), Bool())
# validate type cast to bool
blank = Bool()
b = StringIO()
Bool(True).serialize(b)
blank.deserialize(b.getvalue())
self.assert_(blank.data)
self.assert_(type(blank.data) == bool)
b = StringIO()
Bool(True).serialize(b)
blank.deserialize(b.getvalue())
self.assert_(blank.data)
self.assert_(type(blank.data) == bool)
def cbJoy(self, joy_msg): model_state_msg = Twist() model_state_msg.linear.x = joy_msg.axes[1] * self.v_gain model_state_msg.linear.y = 0 model_state_msg.linear.z = 0 model_state_msg.angular.x = 0 model_state_msg.angular.y = 0 model_state_msg.angular.z = -joy_msg.axes[3] * self.w_gain self.pub_gazebo.publish(model_state_msg) model_state_msg.linear.x = 0 model_state_msg.angular.z = 0 if joy_msg.buttons[0] == 1: model_state_msg.angular.z = -1 elif joy_msg.buttons[1] == 1: model_state_msg.angular.z = 1 self.pub_gazebo_gripper.publish(model_state_msg) if (joy_msg.buttons[7] == 1): reset_msg = Bool() reset_msg.data = True self.pub_encoder_reset.publish(reset_msg)
def errorCheck(pub): r = rospy.Rate(10) global count precount = 0 thread_unloop_count = 0 while True: error_flag = Bool() if (count - precount) > 0: thread_unloop_count = 0 precount = count elif (count - precount) == 0: thread_unloop_count += 1 precount = count else: thread_unloop_count = 0 precount = 0 if thread_unloop_count > 2: error_flag.data = True print'********++++++++++------------------AMU is rebooting now!!------------------++++++++******' else: error_flag.data = False #commented out by hagiwara 0224 #print'********++++++++++------------------AMU is OK!!------------------++++++++******' pub.publish(error_flag) r.sleep()
def talker(options):
pub = rospy.Publisher("pr2_etherCAT/motors_halted", Bool)
rospy.init_node(NAME, anonymous=True)
while not rospy.is_shutdown():
out = Bool()
out.data = options.halt
pub.publish(out)
rospy.sleep(1.0)
def publishOrderBegun():
pubBegunMsg = Bool()
if(orderProcess == True):
pubBegunMsg.data = 1
else:
pubBegunMsg.data = 0
global pubOrderBegun
pubOrderBegun.publish(pubBegunMsg)
def pub_e( self , enable ):
""" pub actuator cmd """
msg = Bool()
msg.data = enable
self.pub_enable.publish( msg )
def vacuum_state_cb(msg, arm):
print("state call back %8s %7.2f %s"%(arm, g_force[arm], msg.data))
pub_msg = Bool(False)
if g_force[arm] > 15 and msg.data is True:
pub_msg.data = True
if arm == "left":
left_vacuum_pub.publish(pub_msg)
elif arm == "right":
right_vacuum_pub.publish(pub_msg)
def send_move_status(msg):
"""
Send a movement status message.
:param msg: A bool, true if moving, false if stopped.
"""
global move_status_pub
status_msg = Bool()
status_msg.data = msg
move_status_pub.publish(status_msg)
def halt_test():
rospy.init_node('halt_test')
bool_msg = Bool()
bool_msg.data = True
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub_halt = rospy.Publisher('/collide/halt', Bool, queue_size = 10)
print 'published successfully!'
rate.sleep()
pub_halt.publish(bool_msg)
def shutdown(self):
rospy.loginfo("Stopping the robot...")
# Stop the robot
self.cmd_vel_pub.publish(Twist())
rospy.loginfo("enable BarDetectFlag")
# enable BarDetectFlag
flag=Bool()
flag.data=True
self.barDetectFlag_pub.publish(flag)
rospy.sleep(0.5)
def run_dryer(self, run_it=True):
cmd = Bool()
if run_it == True:
cmd.data = True
else:
cmd.data = False
"Updating dryer"
self.dryer_pub.publish(cmd)
def _stop_teleop_cb(self): ''' Sends the stop teleop command IGC ''' msg = Bool() msg.data = True ret = QMessageBox.question(self._widget, "Stop Arm Teleop", 'Do you want to stop the arm teleoperation?', QMessageBox.Ok, QMessageBox.Cancel) if ret == QMessageBox.Ok: self.teleop_done_pub.publish(msg)
def laser_callback(self, laser_scan):
if self.ping_index_ < 0:
self.set_scan_properties(laser_scan)
# Assume min/max angle are symmetrical and set up evenly on robot
# This is the ping index that is straight ahead
self.ping_index_ = int(
(0 - self.angle_min_) / self.angle_increment_)
rospy.loginfo("LIDAR setup: ping_index = %d", self.ping_index_)
# Set the beginning and end of our scan zone
self.min_index = self.ping_index_ - int(self.ALARM_SCAN_WIDTH / 2)
if self.min_index < 0:
rospy.logwarn("Lidar alarm scan width is too large!")
self.min_index = 0
self.max_index = self.ping_index_ + int(self.ALARM_SCAN_WIDTH / 2)
if self.max_index > len(laser_scan.ranges):
rospy.logwarn("Lidar alarm scan width is too large")
self.max_index = len(laser_scan.ranges)
rospy.loginfo("min index is %d", self.min_index)
rospy.loginfo("max index is %d", self.max_index)
count = 0
num_pings = 0
average = 0
for x in range(self.min_index, self.max_index):
if laser_scan.ranges[x] > self.range_min_:
if laser_scan.ranges[x] < self.range_max_:
num_pings += 1
rospy.logdebug("Ping %d with range %f", num_pings,
laser_scan.ranges[x])
average += laser_scan.ranges[x]
if laser_scan.ranges[x] < self.MIN_SAFE_DISTANCE:
count += 1
rospy.loginfo(
"%d valid lidar pings, %d dangerous pings, average distance %f",
num_pings, count, float(average) / float(num_pings))
if count > (num_pings * self.ALARM_TRIP_PERCENTAGE):
self.laser_alarm = True
rospy.loginfo("LIDAR ALARM! DANGER!")
else:
self.laser_alarm = False
lidar_alarm_msg = Bool()
lidar_alarm_msg.data = self.laser_alarm
self.lidar_alarm_publisher.publish(lidar_alarm_msg)
lidar_dist_msg = Float32()
lidar_dist_msg.data = self.ping_dist_in_front_
self.lidar_dist_publisher_.publish(lidar_dist_msg)
def on_stopButton_clicked(self):
try:
rospy.delete_param('/tracking/scenario')
except KeyError:
pass
msg1 = SimControl()
msg1.mode = "stop"
self.pub_control.publish(msg1)
msg2 = Bool()
msg2.data = True
self.pub_reset.publish(msg2)
def pauseSystem(self, pausing):
learningStatus = Bool()
controlStatus = Bool()
if not pausing:
self.expStatus = "monitor"
self.backToInit = not pausing
learningStatus.data = not pausing
self.learningMF_pub.publish(learningStatus)
self.learningMB_pub.publish(learningStatus)
self.learningMB2_pub.publish(learningStatus)
controlStatus.data = not pausing
self.control_pub.publish(controlStatus)
def __init__(self):
# initialize robot node
rospy.init_node("robot")
# parse config file
self.config = read_config()
self.goal = self.config['goal']
self.threshold = self.config['threshold']
# initialize temperature and texture fields
self.temperature = 0.0
self.texture = ''
self.position = [0,0]
self.move = ''
# initialize robot's belief about position
initial_belief = 1/(float((len(self.config['pipe_map']) * len(self.config['pipe_map'][0]))) - float(len(self.config['walls'])))
self.prior_belief = [[initial_belief for x in range(len(self.config['pipe_map'][y]))] for y in range(len(self.config['pipe_map']))]
for x, row in enumerate(self.prior_belief):
for y, col in enumerate(row):
if [x,y] in self.config['walls']:
col = 0.0
#calculate MDP here
self.policy = mdp(self.config)
self.move_instruction = {
'N': [-1,0],
'W': [0,-1],
'S': [1,0],
'E': [0,1],
'WALL': [2,2],
'GOAL': [0,0]
}
# Initialize subscribers, publishers, and services
self.init_ros_things()
# Publish bool message to temp sensor's activation topic
bool_msg = Bool()
bool_msg.data = True
self.temperature_activation.publish(bool_msg)
# Publish the results of MDP
policy_list = [x for y in self.policy for x in y]
self.policy_publisher.publish(policy_list)
rospy.sleep(1)
rospy.spin()
rospy.signal_shutdown(self)
def haltCB(msg):
global HALT
#print "haltCB start"
HALT = msg.data
#pub_twist = rospy.Publisher('/robot0/cmd_vel', Twist, queue_size = 10)
pub_turn_reset = rospy.Publisher('/turn/reset', Bool, queue_size = 10)
if HALT == True:
#once halt, send reset to the turn
bool_msg = Bool()
twist = Twist()
bool_msg.data = True
pub_turn_reset.publish(bool_msg)
print "turn reset message was published successfully in haltCB!"
def _handle_vision_ball_position(msg):
global _measured, _last_time
_measured = (msg.x, msg.y)
# Timestamp this msg so the ball updater knows
# how much time elapsed since last measurement
_last_time = time.time()
# Was there a goal?
if abs(msg.x) > 1.79 and (abs(msg.y) < .305):
msg = Bool()
msg.data = True
_goal_pub.publish(msg)
def get_ctrl_output(self):
# if in velocity control mode, just return the goal velocity
if self.ctrl_mode == 'open':
cmd_vel = Twist()
cmd_vel.linear.x = self.vel_g[0]
cmd_vel.angular.y = self.vel_g[1]
goal_reached = Bool()
goal_reached.data = False
return cmd_vel, goal_reached
try:
# update position information
(translation, rotation) = self.trans_listener.lookupTransform(
"/map", "/base_footprint", rospy.Time(0))
self.x = translation[0]
self.y = translation[1]
self.theta = tf.transformations.euler_from_quaternion(rotation)[2]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr("Cannot localize robot!")
# use self.x self.y and self.theta to compute the right control input here
# compute rho and alpha, beta
x_g = self.x_g
y_g = self.y_g
th_g = self.th_g
# Distance to goal
rho = np.sqrt((x_g - self.x)**2 + (y_g - self.y)**2)
# Direction to goal
ang = np.arctan2(y_g - self.y, x_g - self.x)
# Difference between heading and ang
alpha = self.wrapToPi(ang - self.theta)
# Difference between heading and th_g
beta = self.wrapToPi(th_g - self.theta)
# check if near goal
# Only reset if double the threshold from goal. Adds robustness to map jumping
if not self.near_goal:
self.near_goal = rho < self.goal_reached_thresh
else:
self.near_goal = 0.5 * rho < self.goal_reached_thresh
if not self.near_goal:
V = 0.5 - np.absolute(np.arctan(alpha * 5.0) /
(np.pi)) - max(0, -0.5 / 0.2 * rho + 0.5)
om = np.arctan(alpha * 2.5) / (np.pi / 2.0)
else:
V = 0
om = np.arctan(beta * 2.5) / (np.pi / 2.0)
# Apply saturation limits
V = np.sign(V) * min(0.3, np.abs(V))
om = np.sign(om) * min(1, np.abs(om))
cmd = Twist()
cmd.linear.x = V
cmd.angular.z = om
# check if goal is reached
goal_reached = Bool()
near_theta = np.absolute(self.theta - th_g) < self.theta_goal_thresh
goal_reached.data = self.near_goal and near_theta
#if np.linalg.norm(rho) < self.goal_reached_thresh and (np.absolute(self.theta - th_g) < self.theta_goal_thresh):
# goal_reached.data = True
#else:
# goal_reached.data = False
return cmd, goal_reached
def openDoor(self):
self.openDoorStatus = not self.openDoorStatus
status = Bool()
status.data = self.openDoorStatus
self.set_doorstate_publisher.publish(status)
def using_teleop_timer(self, event):
if not rospy.is_shutdown():
self.pub_using_telop.publish(Bool(self.using_teleop))
def publish_lc_status(self, msg):
self.publishers['lc_status'].publish(Bool(msg))
def __init__(self):
# The current status of the joints.
self.JointState = JointTrajectoryControllerState()
# The servo power's status of the robot.
self.ServoPowerState = Bool()
# The fault power's status of the robot.
self.PowerFaultState = Bool()
# The reference coordinate in the calculations of the elfin_basic_api node
self.RefCoordinate = String()
# The end coordinate in the calculations of the elfin_basic_api node
self.EndCoordinate = String()
#The value of the dynamic parameters of elfin_basic_api, e.g. velocity scaling.
self.DynamicArgs = Config()
# get the reference coordinate name of elfin_basic_api from the response of this service.
self.call_ref_coordinate = rospy.ServiceProxy('elfin_basic_api/get_reference_link', SetBool)
self.call_ref_coordinate_req = SetBoolRequest()
# get the current position of elfin_ros_control from the response of this service.
self.call_current_position = rospy.ServiceProxy('elfin_ros_control/elfin/get_current_position', SetBool)
self.call_current_position_req = SetBoolRequest()
# call service recognize_position of elfin_ros_control.
self.call_recognize_position = rospy.ServiceProxy('elfin_ros_control/elfin/recognize_position', SetBool)
self.call_recognize_position_req = SetBoolRequest()
self.call_recognize_position_req.data = True
# get the end coordinate name of elfin_basic_api from the response of this service.
self.call_end_coordinate = rospy.ServiceProxy('elfin_basic_api/get_end_link', SetBool)
self.call_end_coordinate_req = SetBoolRequest()
# for publishing joint goals to elfin_basic_api
self.JointsPub = rospy.Publisher('elfin_basic_api/joint_goal', JointState, queue_size=1)
self.JointsGoal = JointState()
# for publishing cart goals to elfin_basic_api
self.CartGoalPub = rospy.Publisher('elfin_basic_api/cart_goal', PoseStamped, queue_size=1)
self.CartPos = PoseStamped()
# for pub cart path
self.CartPathPub = rospy.Publisher('elfin_basic_api/cart_path_goal', PoseArray, queue_size=1)
self.CartPath = PoseArray()
self.CartPath.header.stamp=rospy.get_rostime()
self.CartPath.header.frame_id='elfin_base_link'
# for pub one specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointCmdPub = rospy.Publisher('elfin_teleop_joint_cmd_no_limit', Int64 , queue_size=1)
self.JointCmd = Int64()
# for pub multi specific joint action to elfin_teleop_joint_cmd_no_limit
self.JointsCmdPub = rospy.Publisher('changyuan_joints_cmd', JointsFloat64, queue_size=1)
self.JointsCmd = JointsFloat64()
# action client, send goal to move_group
self.action_client = SimpleActionClient('elfin_module_controller/follow_joint_trajectory',
FollowJointTrajectoryAction)
self.action_goal = FollowJointTrajectoryGoal()
#self.goal_list = JointTrajectoryPoint()
self.goal_list = []
self.joints_ = []
self.ps_ = []
self.listener = tf.TransformListener()
self.robot=moveit_commander.RobotCommander()
self.scene=moveit_commander.PlanningSceneInterface()
self.group=moveit_commander.MoveGroupCommander('elfin_arm')
self.ref_link_name=self.group.get_planning_frame()
self.end_link_name=self.group.get_end_effector_link()
self.ref_link_lock=threading.Lock()
self.end_link_lock=threading.Lock()
self.call_teleop_stop=rospy.ServiceProxy('elfin_basic_api/stop_teleop', SetBool)
self.call_teleop_stop_req=SetBoolRequest()
self.call_teleop_joint=rospy.ServiceProxy('elfin_basic_api/joint_teleop',SetInt16)
self.call_teleop_joint_req=SetInt16Request()
self.call_teleop_joints=rospy.ServiceProxy('elfin_basic_api/joints_teleops',SetFloat64s)
self.call_teleop_joints_req=SetFloat64sRequest()
self.call_teleop_cart=rospy.ServiceProxy('elfin_basic_api/cart_teleop', SetInt16)
self.call_teleop_cart_req=SetInt16Request()
self.call_move_homing=rospy.ServiceProxy('elfin_basic_api/home_teleop', SetBool)
self.call_move_homing_req=SetBoolRequest()
self.call_reset=rospy.ServiceProxy(self.elfin_driver_ns+'clear_fault', SetBool)
self.call_reset_req=SetBoolRequest()
self.call_reset_req.data=True
self.call_power_on = rospy.ServiceProxy(self.elfin_driver_ns+'enable_robot', SetBool)
self.call_power_on_req=SetBoolRequest()
self.call_power_on_req.data=True
self.call_power_off = rospy.ServiceProxy(self.elfin_driver_ns+'disable_robot', SetBool)
self.call_power_off_req = SetBoolRequest()
self.call_power_off_req.data=True
self.call_velocity_setting = rospy.ServiceProxy('elfin_basic_api/set_velocity_scale', SetFloat64)
self.call_velocity_req = SetFloat64Request()
self._velocity_scale = 0.78
self.set_velocity_scale(self._velocity_scale)
pass
#!/usr/bin/env python
import rospy
from std_msgs.msg import Bool
#init ros node
pub = rospy.Publisher('led_status', Bool, queue_size=10)
rospy.init_node('publish', anonymous=True)
Xau = Bool()
Xau.data = True
while True:
print(Xau.data)
pub.publish(Xau)
rospy.sleep(2)
Xau.data = not Xau.data
rospy.loginfo("DONE")
last_time_steps = np.append(last_time_steps, [int(t + 1)])
reward_msg = RLExperimentInfo()
reward_msg.episode_number = x
reward_msg.episode_reward = cumulated_reward
reward_pub.publish(reward_msg)
break
m, s = divmod(int(time.time() - start_time), 60)
h, m = divmod(m, 60)
rospy.loginfo(
("EP: " + str(x + 1) + " - [alpha: " +
str(round(qlearn.alpha, 2)) + " - gamma: " +
str(round(qlearn.gamma, 2)) + " - epsilon: " +
str(round(qlearn.epsilon, 2)) + "] - Reward: " +
str(cumulated_reward) + " Time: %d:%02d:%02d" % (h, m, s)))
done_msg = Bool()
done_msg.data = True
done_pub.publish(done_msg)
l = last_time_steps.tolist()
l.sort()
rospy.loginfo("Overall score: {:0.2f}".format(last_time_steps.mean()))
rospy.loginfo("Best 100 score: {:0.2f}".format(
reduce(lambda x, y: x + y, l[-100:]) / len(l[-100:])))
env.close()
# Save the q_value for latter usage.
np.save(outdir + '/q_value' + str(experiment) + '.npy', qlearn.Q)
def test_std_msgs_Bool(self):
from std_msgs.msg import Bool
self.assertEquals(Bool(), Bool())
self._test_ser_deser(Bool(), Bool())
# default value should be False
self.assertEquals(False, Bool().data)
# test various constructor permutations
for v in [True, False]:
self.assertEquals(Bool(v), Bool(v))
self.assertEquals(Bool(v), Bool(data=v))
self.assertEquals(Bool(data=v), Bool(data=v))
self.assertNotEquals(Bool(True), Bool(False))
self._test_ser_deser(Bool(True), Bool())
self._test_ser_deser(Bool(False), Bool())
# validate type cast to bool
blank = Bool()
b = StringIO()
Bool(True).serialize(b)
blank.deserialize(b.getvalue())
self.assert_(blank.data)
self.assert_(type(blank.data) == bool)
b = StringIO()
Bool(True).serialize(b)
blank.deserialize(b.getvalue())
self.assert_(blank.data)
self.assert_(type(blank.data) == bool)
def run(self):
self.currentState == SPEECH_STATE
while (self.currentState == START_STATE or self.currentState
== SPEECH_STATE) and not rospy.is_shutdown():
pass
self.speech_enable.publish(Bool(False))
def _movo_teleop(self, joyMessage):
self._parse_joy_input(joyMessage)
if self.button_state[MAP_REC_GOAL_IDX] == 1:
if (False == self.goalrecorded):
temp = Bool()
temp.data = True
self.goalrecorder_pub.publish(temp)
self.goalrecorded = True
else:
self.goalrecorded = False
if self.button_state[MAP_DTZ_IDX]:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = DTZ_REQUEST
elif self.button_state[MAP_PWRDWN_IDX]:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = STANDBY_REQUEST
elif self.button_state[MAP_STANDBY_IDX]:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = STANDBY_REQUEST
elif self.button_state[MAP_TRACTOR_IDX]:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_SET_OPERATIONAL_MODE'
self.cfg_cmd.gp_param = TRACTOR_REQUEST
else:
self.cfg_cmd.gp_cmd = 'GENERAL_PURPOSE_CMD_NONE'
self.cfg_cmd.gp_param = 0
if ('GENERAL_PURPOSE_CMD_NONE' != self.cfg_cmd.gp_cmd):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = True
elif (True == self.send_cmd_none):
self.cfg_cmd.header.stamp = rospy.get_rostime()
self.cfg_pub.publish(self.cfg_cmd)
self.cfg_cmd.header.seq
self.send_cmd_none = False
elif (False == self.send_cmd_none):
if self.button_state[MAP_DEADMAN_IDX]:
self.motion_cmd.linear.x = (
self.axis_value[MAP_TWIST_LIN_X_IDX] *
self.x_vel_limit_mps)
self.motion_cmd.linear.y = (
self.axis_value[MAP_TWIST_LIN_Y_IDX] *
self.y_vel_limit_mps)
self.motion_cmd.angular.z = (
self.axis_value[MAP_TWIST_ANG_Z_IDX] *
self.yaw_rate_limit_rps)
self.last_motion_command_time = rospy.get_time()
else:
self.motion_cmd.linear.x = 0.0
self.motion_cmd.linear.y = 0.0
self.motion_cmd.angular.z = 0.0
dt = rospy.get_time() - self.last_joy
self.last_joy = rospy.get_time()
if (dt >= 0.01):
self.limited_cmd.linear.x = slew_limit(
self.motion_cmd.linear.x, self.limited_cmd.linear.x,
self.accel_lim, dt)
self.limited_cmd.linear.y = slew_limit(
self.motion_cmd.linear.y, self.limited_cmd.linear.y,
self.accel_lim, dt)
self.limited_cmd.angular.z = slew_limit(
self.motion_cmd.angular.z, self.limited_cmd.angular.z,
self.yaw_accel_lim, dt)
if ((rospy.get_time() - self.last_motion_command_time) < 2.0):
self.motion_pub.publish(self.limited_cmd)
if self.button_state[MAP_DEADMAN_IDX] and self.button_state[
MAP_MAN_OVVRD_IDX]:
self.override_pub.publish(self.motion_cmd)
def set_autopilot(self, enable):
"""
enable/disable the autopilot
"""
self.auto_pilot_enable_publisher.publish(Bool(data=enable))
def set_vehicle_control_manual_override(self, enable):
"""
Set the manual control override
"""
self.hud.notification('Set vehicle control manual override to: {}'.format(enable))
self.vehicle_control_manual_override_publisher.publish((Bool(data=enable)))
def announceExplorer(self):
object_msg = Bool()
object_msg.data = True
self.explore_pub.publish(object_msg)
def __init__(self):
# publisheri
self.pub1 = rospy.Publisher('/sphero_1/cmd_vel', Twist, queue_size=1)
self.pub2 = rospy.Publisher('/sphero_2/cmd_vel', Twist, queue_size=1)
# stvaramo objekte
self.vel = Twist()
self.pose_m = Point()
# varijable i inicijalne vrijednosti
self.vel.angular.x = 0
self.vel.angular.y = 0
self.pose_m.x = 0
self.pose_m.y = 0
self.pose_x_R_S = 0
self.pose_y_R_S = 0
self.pose_x_B_S = 0
self.pose_y_B_S = 0
self.pose_R_x = 0
self.pose_R_y = 0
self.pose_B_x = 0
self.pose_B_y = 0
# parametri regulatora
self.K_p = 0.5 # proporcionalno pojacanje
self.K_i = 0.0005 # integralno pojacanje
self.K_d = 0 # derivacijsko pojacanje
self.T = 0.15
self.max = 60
self.min = 10
self.v_x = 0
self.v_y = 0
# pomocni brojac
self.cnt = 0
# objekt razreda PID_controller
self.pid_sphero_x = PID_controller(self.K_p, self.K_i, self.K_d,
self.T, self.min, self.max)
self.pid_sphero_y = PID_controller(self.K_p, self.K_i, self.K_d,
self.T, self.min, self.max)
# # dobivanje pozicije crvenog markera --> samo jednom
# self.pose_m = rospy.wait_for_message("/red_m", Point)
# self.pose_R_x = 100.0 * self.pose_m.x
# self.pose_R_y = 100.0 * self.pose_m.y
# # dobivanje pozicije plavog markera --> samo jednom
# self.pose_m = rospy.wait_for_message("/blue_m", Point)
# self.pose_B_x = 100.0 * self.pose_m.x
# self.pose_B_y = 100.0 * self.pose_m.y
self.sphero_start_bool = Bool()
self.sphero_start_bool = False
# subscriberi
rospy.Subscriber("/red_s", Point, self.sphero_red_callback)
rospy.Subscriber("/blue_s", Point, self.sphero_blue_callback)
rospy.Subscriber("/sphero", Bool, self.sphero_start_callback)
rospy.Subscriber("/red_m", Point, self.red_marker)
rospy.Subscriber("/blue_m", Point, self.blue_marker)
# konacna brzina
self.final_value = 0
def switch_position_control(self):
msg = Bool()
msg.data = True
self.switch_pub.publish(msg)
rospy.loginfo("Switched to position control")
# Remove models from the scene on shutdown
# rospy.on_shutdown(delete_gazebo_models)
# Wait for the All Clear from emulator startup
# rospy.wait_for_message("/robot/sim/started", Empty)
limb = 'left'
#shapes = ['triangle', 'square', 'circle']
shapes = ['triangle', 'square']
hover_distance = 0.15
desired_position = [320, 250]
for shape in shapes:
print(shape)
pnp = PickAndPlace(shape, desired_position, limb, hover_distance)
pub_pid_enable = rospy.Publisher("/pid_enable", Bool, queue_size=1)
msg_pid_enable = Bool(True)
pub_pid_enable.publish(msg_pid_enable)
# Subscribe Image from camera
rospy.Subscriber("/detected_object", Object, pnp.callback_camera)
# Subscribe Image from camera
# rospy.Subscriber("/control_effort", Float64, pnp.callback_control_effort)
# Subscribe Image from kinect
#rospy.Subscriber("/xxxxxxx", Pose, pnp.callback_kinect)
quaternion = tf.transformations.quaternion_from_euler(0.0,
math.pi,
0.0,
axes='sxyz')
hover = Pose()
lightPubs = {}
addressMap = {}
for robot in robotNames:
key = str((len(robotKeyNameMap) + 1) % 10)
robotKeyNameMap[key] = robot
velPubs[key] = rospy.Publisher(robot + '/cmd_vel_relay',
Twist,
queue_size=1)
commPubs[key] = rospy.Publisher(robot + '/comm', String, queue_size=1)
selPubs[key] = rospy.Publisher(robot + '/select', Bool, queue_size=1)
lightPubs[key] = rospy.Publisher(robot + '/light', Bool, queue_size=1)
addressMap[key] = robotAddressMap[robot]
if len(robotKeyNameMap) == 10:
break
currentRobotKey = '1'
flag = Bool()
flag.data = True
selPubs[currentRobotKey].publish(flag)
speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
x = 0
y = 0
z = 0
th = 0
status = 0
try:
print(msg)
print('--------------------------')
print('Robot List:')
def run(self):
rate = rospy.Rate(5)
while not rospy.is_shutdown():
self.rescue_pub.publish(Bool(self.rescuing))
rate.sleep()
def run(self):
f_r = open("trajectories.txt", "w+")
key = ""
heading = Float32()
heading.data = 0.0
distance = Float32()
return_ground_truth = Bool()
return_ground_truth.data = True
while key != 's':
modelstate = self.get_model_state(model_name="mobile_base")
currx = modelstate.pose.position.x
curry = modelstate.pose.position.y
roll = modelstate.pose.orientation.x
pitch = modelstate.pose.orientation.y
yaw = modelstate.pose.orientation.z
w = modelstate.pose.orientation.w
orientation_list = [roll, pitch, yaw,w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
print "current positions are: "+str((currx,curry,yaw))
key = raw_input("PRESS CONTROL KEYS:\n(The rotation keys rotate the turtlebot with respect to x-axis)\nl : +45 degree\na : -45 degree\nt : +90 degree\nv : -90 degree\nj : 0 degree\nf : -180 degree\nh : +135 degree\ng: -135 degree\n\nd : to move 1 cm\nm : to move sqrt(2) cm (diagonally)\n\ns : to stop\n")
distance.data = 0.0
if key == 'l':
heading.data = np.pi/4
elif key == 'a':
heading.data = -np.pi/4
elif key == 't':
heading.data = np.pi/2
elif key == 'v':
heading.data = -np.pi/2
elif key == 'j':
heading.data = 0
elif key == 'f':
heading.data = -np.pi
elif key == 'h':
heading.data = 3*np.pi/4
elif key == 'g':
heading.data = -3*np.pi/4
elif key == 'd':
distance.data = 1.0
elif key == 'm':
distance.data = 1.414
print("Heading: "+str(heading))
print("Distance: "+str(distance))
f_r.write("Heading: "+str(heading)+"\n")
f_r.write("Distance: "+str(distance)+"\n")
output = self.turtlebot_control_service(heading, distance, return_ground_truth)
#print(output)
f_r.write(str(output)+"\n")
f_r.close()
rospy.spin()
if __name__ == '__main__':
try:
rospy.init_node('set_goal')
done_pub = rospy.Publisher('/elevator/done', Bool, queue_size=1)
move = Navigation()
move.set_init_pose(pos=[-24.5, -3.5, 0],
ori=[0, 0, 0.999993843881, 0.00350887452617],
cov=[
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.06853892326654787
])
# Go to cafe
result = move.movebase_client(
pos=[-49.2158203125, -7.58403015137, 0],
ori=[0, 0, 0.708139986808, 0.706072063661])
### DO STUFF ###
# Go back to elevator
result = move.movebase_client(
pos=[-25.4651012421, -3.54517769814, 0],
ori=[0, 0, 0.0123519308175, 0.999923711993])
done_pub.publish(Bool(data=True))
except rospy.ROSInterruptExsception:
rospy.loginfo("Navigation test finished.")
from cv_bridge import CvBridge
from std_msgs.msg import String
from std_msgs.msg import UInt32MultiArray, UInt32, Bool
#Others
import sys
import os
import numpy as np
import matplotlib.pyplot as plt
from threading import Thread
#MoPAT alogirithm files
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../")
from algorithms.mopat_astar_ros import Astar
#Global variables
motion_plans_done = Bool() #Flag - Bool type rosmsg
static_config = None #Predefined global static_config
screen_size = None #Predefined global static_config
robot_num = 0 #Number of robots in simulation
robot_starts = {} #Dict - robot_index : robot_start
robot_goals = {} #Dict - robot_index : robot_goal
motion_plans = {} #Dict - robot_index : robot_plan
got_static_config = False #Flag - True if static_config received
all_planners_started = False #Flag - True if all motion_planners started
motion_plans_done.data = False #Flag - True if all motion plans done
bridge = CvBridge() #Required for rosmsg-cv conversion
def config_space_cb(data):
'''
def publish_dbw_status(self, data):
self.publishers['dbw_status'].publish(Bool(data))
def publish_dbw_status(self, data):
self.publishers['dbw_status'].publish(Bool(data))
self.server('dbw_status', data={'dbw_status': str(data)})
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import time
from std_msgs.msg import Bool, Float32
if __name__ == '__main__':
rospy.init_node('actuator', anonymous=True)
# 吸盘控制
suction_pub = rospy.Publisher('suction_chatter', Bool, queue_size=10)
# 夹爪控制
gripper_pub = rospy.Publisher('gripper_chatter', Bool, queue_size=10)
# 定义Bool变量 并初始化
suction_signal = Bool(data=False)
gripper_signal = Bool(data=False)
# 下面使吸盘和夹爪间歇切换工作
while True:
# 发布False,夹爪张开
gripper_signal.data = False
gripper_pub.publish(gripper_signal)
print("stop")
rospy.sleep(2)
# 发布True，夹爪闭合
gripper_signal.data = True
gripper_pub.publish(gripper_signal)
print("activate")
rospy.sleep(2)
# 发布False,吸盘释放
def doOdomPub():
global camUpdateFlag, carUpdateFlag, mStatusLock
global cam_currentPose, cam_currentTime
global car_currentPose, car_currentTime
global car_lastPose, cam_lastPose, Odom_last
global begin_flag, scale, OdomPub, globalMovePub, velPub
global tf_rot, tf_trans, scale
global odom_combined_tf, odom_combined_tf2
time_now = rospy.Time.now()
Odom = Odometry()
Odom.header.stamp = time_now
Odom.header.frame_id = "/odom_combined"
mStatusLock.acquire()
camflag = camUpdateFlag
carflag = carUpdateFlag
if begin_flag:
cam_lastPose = cam_currentPose
car_lastPose = car_currentPose
if camUpdateFlag:
Tad = np.array([
cam_currentPose.position.x, cam_currentPose.position.y,
cam_currentPose.position.z
])
q = [
cam_currentPose.orientation.x, cam_currentPose.orientation.y,
cam_currentPose.orientation.z, cam_currentPose.orientation.w
]
M = tf.transformations.quaternion_matrix(q)
Rad = M[:3, :3]
#为了简化计算，下文的计算中base_link 和base_footprint被看成是相同的坐标系
#转到odom_combined，得到camera在odom_combined中的pose
Rbd = tf_rot.dot(Rad)
Tbd = scale * (tf_rot.dot(Tad)) + tf_trans
#由camera 的 pose 得到 base_footprint 的pose，这也是下文要发布的pose
Rdc = tf_rot.T
Tdc = -1 / scale * (Rdc.dot(tf_trans))
Rbc = Rbd.dot(Rdc)
Tbc = scale * (Rbd.dot(Tdc)) + Tbd
Odom.pose.pose.position.x = Tbc[0]
Odom.pose.pose.position.y = Tbc[1]
Odom.pose.pose.position.z = 0. #Tbc[2]
M = np.identity(4)
M[:3, :3] = Rbc
q = tf.transformations.quaternion_from_matrix(M)
Odom.pose.pose.orientation.x = q[0]
Odom.pose.pose.orientation.y = q[1]
Odom.pose.pose.orientation.z = q[2]
Odom.pose.pose.orientation.w = q[3]
cam_lastPose = cam_currentPose
else:
if carUpdateFlag:
q = [
Odom_last.pose.pose.orientation.x,
Odom_last.pose.pose.orientation.y,
Odom_last.pose.pose.orientation.z,
Odom_last.pose.pose.orientation.w
]
euler = euler_from_quaternion(q)
theta_last = euler[2]
delta_car = math.sqrt(
(car_currentPose.position.x - car_lastPose.position.x) *
(car_currentPose.position.x - car_lastPose.position.x) +
(car_currentPose.position.y - car_lastPose.position.y) *
(car_currentPose.position.y - car_lastPose.position.y))
if car_currentPose.position.z < 0.000001:
delta_car = -1. * delta_car
Odom.pose.pose.position.x = Odom_last.pose.pose.position.x + delta_car * math.cos(
theta_last)
Odom.pose.pose.position.y = Odom_last.pose.pose.position.y + delta_car * math.sin(
theta_last)
Odom.pose.pose.position.z = 0.
q1 = [
car_lastPose.orientation.x, car_lastPose.orientation.y,
car_lastPose.orientation.z, car_lastPose.orientation.w
]
q2 = [
car_currentPose.orientation.x, car_currentPose.orientation.y,
car_currentPose.orientation.z, car_currentPose.orientation.w
]
euler1 = euler_from_quaternion(q1)
euler2 = euler_from_quaternion(q2)
theta_dleta = euler2[2] - euler1[2]
if abs(theta_dleta) > 0.4:
theta_dleta = 0.0
q = quaternion_from_euler(0, 0, theta_last + theta_dleta)
Odom.pose.pose.orientation.x = q[0]
Odom.pose.pose.orientation.y = q[1]
Odom.pose.pose.orientation.z = q[2]
Odom.pose.pose.orientation.w = q[3]
else:
Odom = Odom_last
time1_diff = time_now - car_currentTime
time2_diff = time_now - cam_currentTime
car_diff = abs(car_currentPose.position.x - car_lastPose.position.x
) + abs(car_currentPose.position.y -
car_lastPose.position.y)
if car_diff > 2.0 or time2_diff.to_sec() > 20. or time1_diff.to_sec(
) > 5.:
#发布全局禁止ｆｌａｇ
globalMoveFlag = Bool()
globalMoveFlag.data = False
globalMovePub.publish(globalMoveFlag)
CarTwist = Twist()
velPub.publish(CarTwist)
car_lastPose = car_currentPose
cam_currentTime = rospy.Time.now()
car_currentTime = rospy.Time.now()
mStatusLock.release()
return
if carUpdateFlag:
car_lastPose = car_currentPose
camUpdateFlag = False
carUpdateFlag = False
if begin_flag:
Odom_last = Odom
begin_flag = False
mStatusLock.release()
#计算速度
Odom.child_frame_id = "/base_footprint"
Odom.twist.twist.linear.x = 30 * (Odom.pose.pose.position.x -
Odom_last.pose.pose.position.x)
Odom.twist.twist.linear.y = 30 * (Odom.pose.pose.position.y -
Odom_last.pose.pose.position.y)
q1 = [
Odom_last.pose.pose.orientation.x, Odom_last.pose.pose.orientation.y,
Odom_last.pose.pose.orientation.z, Odom_last.pose.pose.orientation.w
]
euler1 = euler_from_quaternion(q1)
q2 = [
Odom.pose.pose.orientation.x, Odom.pose.pose.orientation.y,
Odom.pose.pose.orientation.z, Odom.pose.pose.orientation.w
]
euler2 = euler_from_quaternion(q2)
Odom.twist.twist.angular.z = (euler2[2] - euler1[2]) * 30.
OdomPub.publish(Odom)
q = [
Odom.pose.pose.orientation.x, Odom.pose.pose.orientation.y,
Odom.pose.pose.orientation.z, Odom.pose.pose.orientation.w
]
M = tf.transformations.quaternion_matrix(q)
Rbc = M[:3, :3]
Tbc = np.array([
Odom.pose.pose.position.x, Odom.pose.pose.position.y,
Odom.pose.pose.position.z
])
if odom_combined_tf != None:
Rcb = Rbc.T
T = -Rcb.dot(Tbc)
M = np.identity(4)
M[:3, :3] = Rcb
q = tf.transformations.quaternion_from_matrix(M)
odom_combined_tf.sendTransform(T, q, time_now, "/odom_combined",
"/base_footprint")
if odom_combined_tf2 != None:
M = np.identity(4)
M[:3, :3] = tf_rot
q = tf.transformations.quaternion_from_matrix(M)
odom_combined_tf2.sendTransform(tf_trans, q, time_now,
"/ORB_SLAM/World", "/odom_combined")
Odom_last = Odom
def execute(self, userdata):
completed = Bool()
completed.data = True
self.completed_task_publisher.publish(completed)
return 'done'
def __init__(self):
# self.bridge = CvBridge()
# self.image_received = False
self.encLeft_received = False
self.encRight_received = False
self.apriltagStatus_received = False
self.apriltagID_received = False
self.apriltag_detection_ID = None
self.taskONE = False
self.taskTWO = False
self.MAX_LIN_VEL = 0.02
self.MAX_ANG_VEL = 0.03
# set PID values for panning
self.panP = 0.5
self.panI = 0
self.panD = 0
# set PID values for tilting
self.tiltP = 0.5
self.tiltI = 0
self.tiltD = 0.5
# create a PID and initialize it
self.panPID = PID(self.panP, self.panI, self.panD)
self.tiltPID = PID(self.tiltP, self.tiltI, self.tiltD)
self.panPID.initialize()
self.tiltPID.initialize()
self.partyTwist = Twist()
self.resetLeft = Bool()
self.resetRight = Bool()
rospy.logwarn("Party Node [ONLINE]...")
# rospy shutdown
rospy.on_shutdown(self.cbShutdown)
# Subscribe to Int64 msg
self.encLeft_topic = "/val_encLeft"
self.encLeft_sub = rospy.Subscriber(self.encLeft_topic, Int64,
self.cbEncoderLeft)
# Subscribe to Int64 msg
self.encRight_topic = "/val_encRight"
self.encRight_sub = rospy.Subscriber(self.encRight_topic, Int64,
self.cbEncoderRight)
# Subscribe to Bool msg
self.apriltagStatus_topic = "/apriltag_detection_status"
self.apriltagStatus_sub = rospy.Subscriber(
self.apriltagStatus_topic, Bool, self.cbAprilTagDetectionStatus)
# Subscribe to Int64 msg
self.apriltagID_topic = "/apriltag_detection_ID"
self.apriltagID_sub = rospy.Subscriber(self.apriltagID_topic, Int64,
self.cbAprilTagDetectionID)
# Subscribe to objCenter msg
self.objCoord_topic = "/objCoord"
self.objCoord_sub = rospy.Subscriber(self.objCoord_topic, objCoord,
self.cbObjCoord)
# Subscribe to CameraInfo msg
self.telloCameraInfo_topic = "/cv_camera/camera_info_converted"
self.telloCameraInfo_sub = rospy.Subscriber(self.telloCameraInfo_topic,
CameraInfo,
self.cbCameraInfo)
# Publish to Twist msg
self.partyTwist_topic = "/cmd_vel"
self.partyTwist_pub = rospy.Publisher(self.partyTwist_topic,
Twist,
queue_size=10)
# Subscribe to Int64 msg
self.rstEncLeft_topic = "/rstEncLeft"
self.rstEncLeft_pub = rospy.Publisher(self.rstEncLeft_topic,
Bool,
queue_size=10)
# Subscribe to Int64 msg
self.rstEncRight_topic = "/rstEncRight"
self.rstEncRight_pub = rospy.Publisher(self.rstEncRight_topic,
Bool,
queue_size=10)
# Allow up to one second to connection
rospy.sleep(1)
class AvoidPredictedAccident:
accident_predict_result = AccidentPredictResult()
seniorcar_odometry = Odometry()
seniorcar_state = SeniorcarState()
seniorcar_command = SeniorcarState()
seniorcar_command.accel_opening = 0
seniorcar_state.accel_opening = 0
seniorcar_command.steer_angle = 0
seniorcar_command.max_velocity = 2.0
enable_steer_motor = Bool()
enable_steer_motor.data = False #for initialize 11/2
enable_program = Bool() #11/6
enable_program.data = True #11/6 the seniorcar avoids a step then it turns False
start_odometry = Odometry()
odom_recive_flag = True
update_flag = Bool()
update_flag.data = True #11/7_23_15
time_counter = 0 #one way to keep time. 11/7
time_counter_flag = False
t1 = rospy.Time() #another way to keep time.11/7
t2 = rospy.Time()
t3 = 0
t1_flag = Bool()
t1_flag.data = True
avoid_index1 = 0
avoid_index2 = 0
counter = 0
old_steer_command_angle = 0.0
def __init__(self):
rospy.init_node('generate_seniorca_comand_to_avoid_predicted_accident')
self.pub = rospy.Publisher('seniorcar_command',
SeniorcarState,
queue_size=10)
self.pub2 = rospy.Publisher('enable_motor', Bool, queue_size=10)
#self.pub3 = rospy.Publisher('enable_brake', Bool, queue_size=10)#for avoid program colision 11/6
def subscribe_predicted_result(self):
rospy.Subscriber("accident_predict", AccidentPredictResult,
self.accidentsubCallback)
rospy.Subscriber("seniorcar_state", SeniorcarState, self.stateCallback)
rospy.Subscriber("seniorcar_odometry", Odometry, self.odomCallback)
def stateCallback(self, msg):
self.seniorcar_state = msg
self.seniorcar_command.max_velocity = msg.max_velocity
def odomCallback(self, msg):
if self.odom_recive_flag:
self.start_odometry = msg
print "start_odometry"
print "x:" + str(
self.start_odometry.pose.pose.position.x) + "y:" + str(
self.start_odometry.pose.pose.position.y)
self.odom_recive_flag = False
self.seniorcar_odometry = msg
def accidentsubCallback(self, msg):
self.accident_predict_result = msg
self.min_predict_angle = msg.steer_angle[0]
self.predict_angle_devision = abs(msg.steer_angle[1] -
msg.steer_angle[0])
def calcSteerAngleToPredictedAngleIndex(self, steer_angle):
# radに直す
steer_angle *= 3.14 / 180
# まだ判定結果を受信していない場合は0を返す
if len(self.accident_predict_result.steer_angle) < 1:
return 0
# 範囲外の処理
elif steer_angle < self.accident_predict_result.steer_angle[0]:
return 0
elif steer_angle > self.accident_predict_result.steer_angle[-1]:
return int(len(self.accident_predict_result.steer_angle) - 1)
else:
return int(
(steer_angle - self.accident_predict_result.steer_angle[0]) /
(self.accident_predict_result.steer_angle[1] -
self.accident_predict_result.steer_angle[0]))
def update_seniorcar_command(self):
if self.update_flag.data == False:
pass
if self.update_flag.data == True:
data_num = len(self.accident_predict_result.steer_angle)
if data_num < 1:
return
current_steer_index = self.calcSteerAngleToPredictedAngleIndex(
self.seniorcar_state.steer_angle) # 現在の操舵角度0度の番号
if current_steer_index == 0:
current_steer_index += 1
elif current_steer_index == data_num - 1:
current_steer_index -= 1
if self.accident_predict_result.max_distance[
current_steer_index] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
current_steer_index +
1] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
current_steer_index - 1] > APPROACH_THRESHOLD:
# 前方が安全なら操舵角度そのまま
self.seniorcar_command.steer_angle = self.seniorcar_state.steer_angle
self.enable_steer_motor.data = False
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
else:
# 安全に走行できる経路の中で現在の角度に近い場所を探す。そもそも無ければ停止する
avoid_index = -1
for i in range(0, data_num - 1):
if self.accident_predict_result.max_distance[
i] > AVOID_THRESHOLD and self.accident_predict_result.max_distance[
i +
1] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
i - 1] > APPROACH_THRESHOLD:
if abs(current_steer_index -
i) < abs(current_steer_index - avoid_index):
avoid_index = i
if avoid_index == -1:
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening *
DECELERATION_CONSTANT,
MAX_ACCEL_OPENING * DECELERATION_CONSTANT)
self.enable_steer_motor.data = False
else:
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
self.enable_steer_motor.data = True
if avoid_index < 5 and avoid_index > 2: #to adjust offset.11/6
self.avoid_index1 = avoid_index - 2
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
self.avoid_index1] * 180.0 / 3.14
if self.t1_flag.data == True:
self.t1 = rospy.Time.now()
self.t1_flag.data = False
#elif avoid_index == 7:
#self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[avoid_index] * 180.0 /3.14
elif avoid_index > 9 and avoid_index < 12:
self.avoid_index1 = avoid_index + 2
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
self.avoid_index1] * 180.0 / 3.14
if self.t1_flag.data == True:
self.t1 = rospy.Time.now()
self.t1_flag.data = False
else:
self.avoid_index1 = avoid_index
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
self.avoid_index1] * 180.0 / 3.14 #11/6
# 急激に変化しないようにする処理
if abs(self.seniorcar_command.steer_angle -
self.old_steer_command_angle
) > MAX_CHANGE_STEER_ANGLE_BY_STEP:
if self.seniorcar_command.steer_angle > self.old_steer_command_angle:
self.seniorcar_command.steer_angle = self.old_steer_command_angle + MAX_CHANGE_STEER_ANGLE_BY_STEP
elif self.seniorcar_command.steer_angle < self.old_steer_command_angle:
self.seniorcar_command.steer_angle = self.old_steer_command_angle - MAX_CHANGE_STEER_ANGLE_BY_STEP
self.old_steer_command_angle = self.seniorcar_command.steer_angle
def func(self):
pass
def calculate_and_publish_command(self):
rate = rospy.Rate(PUBLISH_RATE)
while not rospy.is_shutdown():
# 最初は足元の情報がないので介入なし
if pow(
self.seniorcar_odometry.pose.pose.position.x -
self.start_odometry.pose.pose.position.x, 2) + pow(
self.seniorcar_odometry.pose.pose.position.y -
self.start_odometry.pose.pose.position.y, 2) < pow(
NO_INTERVATION_DISTANCE, 2):
self.enable_steer_motor.data = False
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
self.seniorcar_command.steer_angle = self.seniorcar_state.steer_angle
else:
self.update_seniorcar_command() #11/7_23_15
if self.t1_flag.data == False:
self.t2 = rospy.Time.now()
self.t3 = self.t2.secs - self.t1.secs
print(self.t3) #to check t3 11/8_10_25
#11/8_9_20 I checked once t1_flag turn False . and then turn off coment out
#11/7_23_15
if self.t3 < DELAY_TIME and self.t3 > 0 or self.t3 == DELAY_TIME: # I will check here .11/7_9_28
self.update_seniorcar_command()
print("Hello")
if self.t1_flag.data == True:
print("True")
#self.func()
elif self.t3 > DELAY_TIME and self.t3 < DELAY_TIME * 2:
self.update_flag.data = False
#self.seniorcar_command.accel_opening = 0
self.avoid_index2 = 14 - self.avoid_index1
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
self.avoid_index2]
print("avoid_index1")
print(self.avoid_index1)
print("avoid_index2")
print(self.avoid_index2)
#self.seniorcar_command.accel_opening = 0
elif self.t3 == 0 or self.t3 < 0:
self.func()
print("start")
else:
print("I'll update")
self.t1_flag.data = True
self.update_flag.data = True #11/7_23_15
self.pub.publish(self.seniorcar_command)
self.pub2.publish(self.enable_steer_motor)
rate.sleep()
def reset(self):
self.force_zero.publish(Bool(True))
import smach_ros
import mavros_msgs
from std_msgs.msg import Bool
from mavros_msgs import srv
from mavros_msgs.srv import SetMode, CommandBool
from geometry_msgs.msg import PoseStamped, TwistStamped
from mavros_msgs.msg import State
from mavros_msgs.msg import Mavlink
ERR = 0.1
goalPose = PoseStamped()
mavPose = PoseStamped()
mavState = State()
repeat = Bool()
repeat.data = True
def MissionFlowCallback(b):
global repeat
repeat = b
def setPoint(x, y, z):
global goalPose
goalPose.pose.position.x = x
goalPose.pose.position.y = y
goalPose.pose.position.z = z
posePub.publish(goalPose)
rate.sleep()