def display_waypoint(self, x, y):
points = Marker()
points.header.frame_id = "odom" # Publish path in map frame
points.type = points.POINTS
points.action = points.ADD
life = Duration()
life.sec = 1000
life.nanosec = 0
points.lifetime = life
points.id = self.goals
points.scale.x = 0.1
points.scale.y = 0.1
points.color.a = 1.0
points.color.r = 0.0
points.color.g = 0.0
points.color.b = 1.0
points.pose.orientation.w = 1.0
point = Point()
point.x = x
point.y = y
points.points.append(point)
# Publish the MarkerArray
self.goals_viz_pub.publish(points)
def test_6_trajectory_illegal(self):
"""Test trajectory server."""
"""This is more of a validation test that the testing suite does the right thing."""
goal = FollowJointTrajectory.Goal()
goal.trajectory.joint_names = [
"elbow_joint",
"shoulder_lift_joint",
"shoulder_pan_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint",
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-0.5 for i in range(6)])
# Create illegal goal by making the second point come earlier than the first
duration_list = [
Duration(sec=6, nanosec=0),
Duration(sec=3, nanosec=0)
]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = duration_list[i]
goal.trajectory.points.append(point)
self.node.get_logger().info("Sending illegal goal")
goal_response = self.call_action(self.jtc_action_client, goal)
self.assertEqual(goal_response.accepted, False)
self.node.get_logger().info("Goal response REJECTED")
def send_goal(self, trajectory, iteration=1):
self.get_logger().info('Waiting for action server...')
self.__client.wait_for_server()
self.__current_trajectory = trajectory
self.__remaining_iteration = iteration - 1
goal_msg = FollowJointTrajectory.Goal()
goal_msg.trajectory.joint_names = trajectory['joint_names']
for point in trajectory['points']:
trajectory_point = JointTrajectoryPoint(
positions=point['positions'],
velocities=point['velocities'],
accelerations=point['accelerations'],
time_from_start=Duration(
sec=point['time_from_start']['sec'],
nanosec=point['time_from_start']['nanosec']))
goal_msg.trajectory.points.append(trajectory_point)
self.get_logger().info('Sending goal request...')
self.__send_goal_future = self.__client.send_goal_async(
goal_msg, feedback_callback=self.__on_feedback_callback)
self.__send_goal_future.add_done_callback(
self.__on_goal_response_callback)
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
self.ns = kwargs.get('ns', str())
self.id = kwargs.get('id', int())
self.type = kwargs.get('type', int())
self.action = kwargs.get('action', int())
from geometry_msgs.msg import Pose
self.pose = kwargs.get('pose', Pose())
from geometry_msgs.msg import Vector3
self.scale = kwargs.get('scale', Vector3())
from std_msgs.msg import ColorRGBA
self.color = kwargs.get('color', ColorRGBA())
from builtin_interfaces.msg import Duration
self.lifetime = kwargs.get('lifetime', Duration())
self.frame_locked = kwargs.get('frame_locked', bool())
self.points = kwargs.get('points', [])
self.colors = kwargs.get('colors', [])
self.text = kwargs.get('text', str())
self.mesh_resource = kwargs.get('mesh_resource', str())
self.mesh_use_embedded_materials = kwargs.get(
'mesh_use_embedded_materials', bool())
def test_7_trajectory_scaled(self):
"""Test robot movement."""
goal = FollowJointTrajectory.Goal()
goal.trajectory.joint_names = [
"elbow_joint",
"shoulder_lift_joint",
"shoulder_pan_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint",
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-1.0 for i in range(6)])
duration_list = [
Duration(sec=6, nanosec=0),
Duration(sec=6, nanosec=500000000)
]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = duration_list[i]
goal.trajectory.points.append(point)
# TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message
# see https://github.com/ros-controls/ros2_controllers/issues/249
# self.node.get_logger().info("Sending scaled goal without time restrictions")
self.node.get_logger().info("Sending goal for robot to follow")
goal_response = self.call_action(self.jtc_action_client, goal)
self.assertEqual(goal_response.accepted, True)
if goal_response.accepted:
result = self.get_result(self.jtc_action_client, goal_response)
self.assertIn(
result.error_code,
(
FollowJointTrajectory.Result.PATH_TOLERANCE_VIOLATED,
FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED,
FollowJointTrajectory.Result.SUCCESSFUL,
),
)
self.node.get_logger().info("Received result")
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
from builtin_interfaces.msg import Duration
self.duration_value = kwargs.get('duration_value', Duration())
from builtin_interfaces.msg import Time
self.time_value = kwargs.get('time_value', Time())
def __init__(self, **kwargs):
assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
"Invalid arguments passed to constructor: %r" % kwargs.keys()
self.transforms = kwargs.get('transforms', list())
self.velocities = kwargs.get('velocities', list())
self.accelerations = kwargs.get('accelerations', list())
from builtin_interfaces.msg import Duration
self.time_from_start = kwargs.get('time_from_start', Duration())
def __init__(self, **kwargs):
assert all('_' + key in self.__slots__ for key in kwargs.keys()), \
'Invalid arguments passed to constructor: %s' % \
', '.join(sorted(k for k in kwargs.keys() if '_' + k not in self.__slots__))
self.transforms = kwargs.get('transforms', [])
self.velocities = kwargs.get('velocities', [])
self.accelerations = kwargs.get('accelerations', [])
from builtin_interfaces.msg import Duration
self.time_from_start = kwargs.get('time_from_start', Duration())
def timer_callback(self):
traj = JointTrajectory()
traj.joint_names = self.joints
point = JointTrajectoryPoint()
point.positions = self.goals[self.i]
point.time_from_start = Duration(sec=4)
traj.points.append(point)
self.publisher_.publish(traj)
self.i += 1
self.i %= len(self.goals)
def test_5_trajectory(self):
"""Test robot movement."""
goal = FollowJointTrajectory.Goal()
goal.trajectory.joint_names = [
"elbow_joint",
"shoulder_lift_joint",
"shoulder_pan_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint",
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-0.5 for i in range(6)])
position_list.append([-1.0 for i in range(6)])
duration_list = [
Duration(sec=6, nanosec=0),
Duration(sec=9, nanosec=0),
Duration(sec=12, nanosec=0),
]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = duration_list[i]
goal.trajectory.points.append(point)
self.node.get_logger().info("Sending simple goal")
goal_response = self.call_action(self.jtc_action_client, goal)
self.assertEqual(goal_response.accepted, True)
if goal_response.accepted:
result = self.get_result(self.jtc_action_client, goal_response)
self.assertEqual(result.error_code,
FollowJointTrajectory.Result.SUCCESSFUL)
self.node.get_logger().info("Received result SUCCESSFUL")
def interp_linear(p0, p1, t_abs):
"""Perform a linear interpolation between two trajectory points."""
t0 = p0.time_from_start.sec + p0.time_from_start.nanosec * 1.0e-6
t1 = p1.time_from_start.sec + p1.time_from_start.nanosec * 1.0e-6
T = t1 - t0
t = t_abs - t0
ratio = max(min((t / T), 1), 0)
q = [0] * len(p0.positions)
qdot = [0] * len(p0.positions)
qddot = [0] * len(p0.positions)
for i in list(range(len(p0.positions))):
q[i] = (1.0 - ratio) * p0.positions[i] + ratio * p1.positions[i]
qdot[i] = (1.0 - ratio) * p0.velocities[i] + ratio * p1.velocities[i]
qddot[i] = (1.0 - ratio) * p0.accelerations[i] + ratio * p1.accelerations[i]
return JointTrajectoryPoint(positions=q, velocities=qdot, accelerations=qddot,
time_from_start=Duration(sec=int(t_abs), nanosec=int(int(t_abs) *
1.0e+6)))
def on_goal(self, goal_handle):
"""Handle a new goal trajectory command."""
# Checks if the joints are just incorrect
for name in goal_handle.trajectory.joint_names:
if name not in self.motors.keys():
self.node.get_logger().warn('Received a goal with incorrect joint names: (%s)' %
', '.join(goal_handle.trajectory.joint_names))
return GoalResponse.REJECT
if not trajectory_is_finite(goal_handle.trajectory):
self.node.get_logger().warn('Received a goal with infinites or NaNs')
return GoalResponse.REJECT
# Checks that the trajectory has velocities
if not has_velocities(goal_handle.trajectory):
self.node.get_logger().warn('Received a goal without velocities')
return GoalResponse.REJECT
# Inserts the current setpoint at the head of the trajectory
now = self.robot.getTime()
positions = []
velocities = []
accelerations = []
for name in goal_handle.trajectory.joint_names:
positions.append(self.position[name])
velocities.append(self.velocity[name])
accelerations.append(0.0)
point0 = JointTrajectoryPoint(
positions=positions,
velocities=velocities,
accelerations=accelerations,
time_from_start=Duration())
goal_handle.trajectory.points.insert(0, point0)
# Add this trajectory to the list if not conflicting with a current trajectory
logger = self.node.get_logger()
for trajectory in self.trajectories:
if bool(set(trajectory.jointTrajectory.joint_names) &
set(goal_handle.trajectory.joint_names)):
logger.info('Goal Refused: a goal sharing some joint is already running')
return GoalResponse.REJECT
trajectory = Trajectory(goal_handle, now)
self.trajectories.append(trajectory)
logger.info('Goal Accepted')
return GoalResponse.ACCEPT
def timer_callback(self):
marker_data = Marker()
marker_array = MarkerArray()
rot = Rotation.from_rotvec(np.array([0, 0, np.pi / 3]))
quo = rot.as_quat()
for i in range(5):
marker_data.header.frame_id = "odom"
#marker_data.header.stamp = rclpy.time
marker_data.ns = "basic_shapes"
marker_data.id = i
marker_data.action = Marker.ADD
marker_data.pose.position.x = 0.0
marker_data.pose.position.y = 0.0
marker_data.pose.position.z = float(i)
marker_data.pose.orientation.x = quo[0]
marker_data.pose.orientation.y = quo[1]
marker_data.pose.orientation.z = quo[2]
marker_data.pose.orientation.w = quo[3]
marker_data.color.r = 1.0
marker_data.color.g = 0.0
marker_data.color.b = 0.0
marker_data.color.a = 1.0
marker_data.scale.x = 0.5
marker_data.scale.y = 0.05
marker_data.scale.z = 0.05
marker_data.lifetime = Duration()
marker_data.type = 0
marker_array.markers.append(marker_data)
self.pub.publish(marker_array)
#self.get_logger().info('Publishing: "%s"' % marker_array.markers[0].pose)
self.get_logger().info('Debug: "%f"' % quo[1])
def spin(self, spin_dist=1.57, time_allowance=10):
self.debug("Waiting for 'Spin' action server")
while not self.spin_client.wait_for_server(timeout_sec=1.0):
self.info("'Spin' action server not available, waiting...")
goal_msg = Spin.Goal()
goal_msg.target_yaw = spin_dist
goal_msg.time_allowance = Duration(sec=time_allowance)
self.info(f'Spinning to angle {goal_msg.target_yaw}....')
send_goal_future = self.spin_client.send_goal_async(
goal_msg, self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Spin request was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def __init__(self, **kwargs):
assert all(['_' + key in self.__slots__ for key in kwargs.keys()]), \
"Invalid arguments passed to constructor: %r" % kwargs.keys()
from std_msgs.msg import Header
self.header = kwargs.get('header', Header())
self.ns = kwargs.get('ns', str())
self.id = kwargs.get('id', int())
self.type = kwargs.get('type', int())
self.action = kwargs.get('action', int())
from geometry_msgs.msg import Point
self.position = kwargs.get('position', Point())
self.scale = kwargs.get('scale', float())
from std_msgs.msg import ColorRGBA
self.outline_color = kwargs.get('outline_color', ColorRGBA())
self.filled = kwargs.get('filled', int())
from std_msgs.msg import ColorRGBA
self.fill_color = kwargs.get('fill_color', ColorRGBA())
from builtin_interfaces.msg import Duration
self.lifetime = kwargs.get('lifetime', Duration())
self.points = kwargs.get('points', list())
self.outline_colors = kwargs.get('outline_colors', list())
def backup(self, backup_dist=0.15, backup_speed=0.025, time_allowance=10):
self.debug("Waiting for 'Backup' action server")
while not self.backup_client.wait_for_server(timeout_sec=1.0):
self.info("'Backup' action server not available, waiting...")
goal_msg = BackUp.Goal()
goal_msg.target = Point(x=float(backup_dist))
goal_msg.speed = backup_speed
goal_msg.time_allowance = Duration(sec=time_allowance)
self.info(
f'Backing up {goal_msg.target.x} m at {goal_msg.speed} m/s....')
send_goal_future = self.backup_client.send_goal_async(
goal_msg, self._feedbackCallback)
rclpy.spin_until_future_complete(self, send_goal_future)
self.goal_handle = send_goal_future.result()
if not self.goal_handle.accepted:
self.error('Backup request was rejected!')
return False
self.result_future = self.goal_handle.get_result_async()
return True
def timer_callback(self):
if self.starting_point_ok:
traj = JointTrajectory()
traj.joint_names = self.joints
point = JointTrajectoryPoint()
point.positions = self.goals[self.i]
point.time_from_start = Duration(sec=4)
traj.points.append(point)
self.publisher_.publish(traj)
self.i += 1
self.i %= len(self.goals)
elif self.check_starting_point and not self.joint_state_msg_received:
self.get_logger().warn(
'Start configuration could not be checked! Check "joint_state" topic!'
)
else:
self.get_logger().warn(
"Start configuration is not within configured limits!")
def interp_cubic(p0, p1, t_abs):
"""Perform a cubic interpolation between two trajectory points."""
t0 = p0.time_from_start.sec + p0.time_from_start.nanosec * 1.0e-6
t1 = p1.time_from_start.sec + p1.time_from_start.nanosec * 1.0e-6
T = t1 - t0
t = t_abs - t0
q = [0] * len(p0.positions)
qdot = [0] * len(p0.positions)
qddot = [0] * len(p0.positions)
for i in list(range(len(p0.positions))):
a = p0.positions[i]
b = p0.velocities[i]
c = (-3 * p0.positions[i] + 3 * p1.positions[i] - 2 * T * p0.velocities[i] -
T * p1.velocities[i]) / T**2
d = (2 * p0.positions[i] - 2 * p1.positions[i] + T * p0.velocities[i] +
T * p1.velocities[i]) / T**3
q[i] = a + b * t + c * t**2 + d * t**3
qdot[i] = b + 2 * c * t + 3 * d * t**2
qddot[i] = 2 * c + 6 * d * t
return JointTrajectoryPoint(positions=q, velocities=qdot, accelerations=qddot,
time_from_start=Duration(sec=int(t_abs), nanosec=int(int(t_abs) *
1.0e+6)))
def timer_callback(self):
# ---------- randmark------------- #
randmark_pos = [1.91, 1.57]
#randmark_pos = self.randmark_position(self.scan_data, self.angle_increment)
self.get_logger().info("true l: %f" % randmark_pos[0] +
", phi: %f" % np.rad2deg(randmark_pos[1]))
"""
#for calculate sigmas
if self.STORE_NUM < 100:
self.store_randmark_pos.append(randmark_pos)
elif self.STORE_NUM == 100:
for i in range(100):
print(self.store_randmark_pos[i][0], ", ")
for i in range(100):
print(self.store_randmark_pos[i][1], ", ")
self.STORE_NUM += 1
"""
"""
# ---------- calculate pose for particle and randmark ---------------#
for i in range(self.PARTICLES_NUM):
true_randmark_pos = [0.0, 1.9, 0.0]
dx = true_randmark_pos[0] - self.particles_x[i]
dy = true_randmark_pos[1] - self.particles_y[i]
d = math.sqrt(dx ** 2 + dy ** 2)
phi = math.atan2(dy, dx) - self.particles_theta[i]
while phi >= np.pi:
phi -= 2 * np.pi
while phi < -np.pi:
phi += 2 * np.pi
particle_suggest_pos = [d, phi]
self.get_logger().info("particle l: %f" % particle_suggest_pos[0] + ", phi: %f" % np.rad2deg(particle_suggest_pos[1]))
###尤度の計算###
distance_dev_rate = 0.001
direction_dev = 0.001
distance_dev = distance_dev_rate * particle_suggest_pos[0]
cov = np.diag(np.array([distance_dev ** 2, direction_dev ** 2]))
self.weights[i] *= multivariate_normal(mean = particle_suggest_pos, cov = cov).pdf(randmark_pos)
print(self.weights[i])
ps = random.choices(self.weights, weights=self.weights, k=self.PARTICLES_NUM ) #wsの要素に比例した確率で、パーティクルをnum個選ぶ
self.weights = [copy.deepcopy(e) for e in ps] # 選んだリストからパーティクルを取り出し、重みを均一に
for p in range(self.PARTICLES_NUM):
self.weights[i] = 1.0/self.PARTICLES_NUM #重みの正規化
"""
# ---------- particles publish------------- #
# calc ideal pose with Dead Reckoning
self.ideal_theta += self.omega * self.DELAT_T
self.ideal_x += self.nu * self.DELAT_T * math.cos(self.ideal_theta)
self.ideal_y += self.nu * self.DELAT_T * math.sin(self.ideal_theta)
markerArray = MarkerArray()
for i in range(self.PARTICLES_NUM):
# add noise
noise_rate_pdf = multivariate_normal(cov=self.C)
ns = noise_rate_pdf.rvs()
noised_nu = self.nu + ns[0] * math.sqrt(
abs(self.nu) / self.DELAT_T) + ns[1] * math.sqrt(
abs(self.omega) / self.DELAT_T)
noised_omega = self.omega + ns[2] * math.sqrt(
abs(self.nu) / self.DELAT_T) + ns[3] * math.sqrt(
abs(self.omega) / self.DELAT_T)
# calc particles pose with Dead Reckoning
self.particles_theta[i] += noised_omega * self.DELAT_T
self.particles_x[i] += noised_nu * self.DELAT_T * math.cos(
self.particles_theta[i])
self.particles_y[i] += noised_nu * self.DELAT_T * math.sin(
self.particles_theta[i])
# convert oira to quo
rot = Rotation.from_rotvec(
np.array([0, 0, self.particles_theta[i]]))
quo = rot.as_quat()
marker = Marker()
marker.id = i
marker.header.frame_id = "/odom"
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.scale.x = 0.5 * self.weights[i] * self.PARTICLES_NUM + 0.001
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.pose.position.x = self.particles_x[i]
marker.pose.position.y = self.particles_y[i]
marker.pose.position.z = 0.1
marker.pose.orientation.x = quo[0]
marker.pose.orientation.y = quo[1]
marker.pose.orientation.z = quo[2]
marker.pose.orientation.w = quo[3]
t = Duration()
marker.lifetime = t
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.a = 1.0
markerArray.markers.append(marker)
self.particles_pub.publish(markerArray)
