Commit e814f312 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

implement conversion from JTraj to ROS msg (#11)

parent cc377ce7
import gcode2contour as gc
import numpy as np
import pickle
from utils import load_plan
import time
import sys
# ROS Libraries
import rospy
from std_msgs.msg import Bool
from trajectory_msgs.msg import (
JointTrajectoryPoint,
JointTrajectory
)
# MOVEit
#import moveit_commander
#from moveit_msgs.msg._RobotTrajectory import RobotTrajectory
## Our libraries
from utils import load_plan, JTraj2ROS
#import gcode2contour as gc
#from extrudex.msg._ExtruderControl import ExtruderControl
#from extrudex.srv._Hotend import Hotend
def execute_plan(plan):
"""
Executes a plan on MoveIt
Single arm only
"""
for t in plan.trajs[0]:
jt = JTraj2ROS(t)
return
if __name__ == '__main__':
......
from .planning_tools import JTrajectory
from .planning_tools import *
from .plan import load_plan
import gcode2contour as gc
import numpy as np
import pickle
from . import JTrajectory
......
"""
Useful functions for planning for the twins
Useful functions
"""
import numpy as np
# ROS Libraries
import rospy
from std_msgs.msg import Bool
from trajectory_msgs.msg import (
JointTrajectoryPoint,
JointTrajectory
)
class JTrajectory(object):
......@@ -42,3 +49,18 @@ def joints_at_time(traj, t):
t2 = traj.time[wp_i]
return (t-t1)/(t2-t1)*(j2-j1) + j1
def JTraj2ROS(jtraj):
"""
convert JTrajectory object to ROS JointTrajectory
"""
j = JointTrajectory()
for i in range(len(jtraj.positions)):
jp = JointTrajectoryPoint()
jp.positions = jtraj.positions[i]
jp.time_from_start = rospy.Duration(jtraj.time[i])
j.points.append(jp)
return j
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment