Plotting callback broken for time param

[mpowelson]

I get this error when running the pick_and_place example with plotting:=true because it has the extra column for time. It's the assert in tesseractTrajectoryToJointTrajectoryMsg. I'm not sure what the easiest way to fix this is. Eventually we need to add some sort of identifier to the variables and stop relying on the number of columns.

Here's the pertinant part of the backtrace

#0  0x00007f558c5da428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
        resultvar = 0
        pd = <optimized out>
        pid = 9891
        selftid = 9891
#1  0x00007f558c5dc02a in __GI_abort () at abort.c:89
        save_stage = 2
        act = {__sigaction_handler = {sa_handler = 0x4, sa_sigaction = 0x4}, sa_mask = {__val = {0, 0, 140733269407824, 47244640256, 140005473824768, 6073256, 667, 6151968, 28091536, 37965936, 140005404218684, 140005405315664, 140005405329312, 0, 140005405315664, 6073256}}, sa_flags = -1870090240, sa_restorer = 0x5caba8}
        sigs = {__val = {32, 0 <repeats 15 times>}}
#2  0x00007f558c5d2bd7 in __assert_fail_base (fmt=<optimized out>, assertion=assertion@entry=0x5caba8 "joint_names.size() == static_cast<unsigned>(traj.cols())", file=file@entry=0x5cab40 "/home/mpowelson/workspaces/temp/src/tesseract/tesseract_ros/include/tesseract_ros/ros_tesseract_utils.h", line=line@entry=667, function=function@entry=0x5ddf20 <tesseract::tesseract_ros::tesseractTrajectoryToJointTrajectoryMsg(trajectory_msgs::JointTrajectory_<std::allocator<void> >&, tesseract::EnvState const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, Eigen::Ref<Eigen::Matrix<double, -1, -1, 1, -1, -1> const, 0, Eigen::OuterStride<-1> > const&)::__PRETTY_FUNCTION__> "void tesseract::tesseract_ros::tesseractTrajectoryToJointTrajectoryMsg(trajectory_msgs::JointTrajectory&, const tesseract::EnvState&, const std::vector<std::__cxx11::basic_string<char> >&, const Eigen::Ref<const Eigen::Matrix<double, -1, -1, 1> >&)") at assert.c:92
        str = 0x246e990 "@\343F\002"
        total = 4096
#3  0x00007f558c5d2c82 in __GI___assert_fail (assertion=0x5caba8 "joint_names.size() == static_cast<unsigned>(traj.cols())", file=0x5cab40 "/home/mpowelson/workspaces/temp/src/tesseract/tesseract_ros/include/tesseract_ros/ros_tesseract_utils.h", line=667, function=0x5ddf20 <tesseract::tesseract_ros::tesseractTrajectoryToJointTrajectoryMsg(trajectory_msgs::JointTrajectory_<std::allocator<void> >&, tesseract::EnvState const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, Eigen::Ref<Eigen::Matrix<double, -1, -1, 1, -1, -1> const, 0, Eigen::OuterStride<-1> > const&)::__PRETTY_FUNCTION__> "void tesseract::tesseract_ros::tesseractTrajectoryToJointTrajectoryMsg(trajectory_msgs::JointTrajectory&, const tesseract::EnvState&, const std::vector<std::__cxx11::basic_string<char> >&, const Eigen::Ref<const Eigen::Matrix<double, -1, -1, 1> >&)") at assert.c:101
No locals.
#4  0x000000000054c12e in tesseract::tesseract_ros::tesseractTrajectoryToJointTrajectoryMsg (traj_msg=..., start_state=..., joint_names=..., traj=...) at /home/mpowelson/workspaces/temp/src/tesseract/tesseract_ros/include/tesseract_ros/ros_tesseract_utils.h:667
        __PRETTY_FUNCTION__ = "void tesseract::tesseract_ros::tesseractTrajectoryToJointTrajectoryMsg(trajectory_msgs::JointTrajectory&, const tesseract::EnvState&, const std::vector<std::__cxx11::basic_string<char> >&, const Eigen::Ref<const Eigen::Matrix<double, -1, -1, 1> >&)"
        jn_to_index = {_M_t = {_M_impl = {<std::allocator<std::_Rb_tree_node<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, int> > >> = {<__gnu_cxx::new_allocator<std::_Rb_tree_node<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, int> > >> = {<No data fields>}, <No data fields>}, _M_key_compare = {<std::binary_function<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, bool>> = {<No data fields>}, <No data fields>}, _M_header = {_M_color = std::_S_red, _M_parent = 0x16b7558, _M_left = 0x7fff0487fa20, _M_right = 0x567bb5 <std::_Sp_counted_base<(__gnu_cxx::_Lock_policy)2>::_M_add_ref_copy()+33>}, _M_node_count = 23819600}}}
#5  0x00000000005578a1 in tesseract::tesseract_ros::ROSBasicPlotting::plotTrajectory (this=0x1b95dc0, joint_names=..., traj=...) at /home/mpowelson/workspaces/temp/src/tesseract/tesseract_ros/include/tesseract_ros/ros_basic_plotting.h:81
        msg = {model_id = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7fff0487fb00 "robot"}, _M_string_length = 5, {_M_local_buf = "robot\000\000\000\000\270F\002\000\000\000", _M_allocated_capacity = 500084928370}}, joint_trajectory = {header = {seq = 0, stamp = {<ros::TimeBase<ros::Time, ros::Duration>> = {sec = 0, nsec = 0}, <No data fields>}, frame_id = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7fff0487fb30 ""}, _M_string_length = 0, {_M_local_buf = '\000' <repeats 15 times>, _M_allocated_capacity = 0}}}, joint_names = {<std::_Vector_base<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >> = {_M_impl = {<std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >> = {<__gnu_cxx::new_allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}, points = {<std::_Vector_base<trajectory_msgs::JointTrajectoryPoint_<std::allocator<void> >, std::allocator<trajectory_msgs::JointTrajectoryPoint_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<trajectory_msgs::JointTrajectoryPoint_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<trajectory_msgs::JointTrajectoryPoint_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}}, multi_dof_joint_trajectory = {header = {seq = 0, stamp = {<ros::TimeBase<ros::Time, ros::Duration>> = {sec = 0, nsec = 0}, <No data fields>}, frame_id = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7fff0487fb90 ""}, _M_string_length = 0, {_M_local_buf = '\000' <repeats 15 times>, _M_allocated_capacity = 0}}}, joint_names = {<std::_Vector_base<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >> = {_M_impl = {<std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >> = {<__gnu_cxx::new_allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}, points = {<std::_Vector_base<trajectory_msgs::MultiDOFJointTrajectoryPoint_<std::allocator<void> >, std::allocator<trajectory_msgs::MultiDOFJointTrajectoryPoint_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<trajectory_msgs::MultiDOFJointTrajectoryPoint_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<trajectory_msgs::MultiDOFJointTrajectoryPoint_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}}, trajectory_start = {name = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7fff0487fbe0 ""}, _M_string_length = 0, {_M_local_buf = '\000' <repeats 15 times>, _M_allocated_capacity = 0}}, urdf_name = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7fff0487fc00 "robot"}, _M_string_length = 5, {_M_local_buf = "robot\000\345\277\000\000\000\000\000\000\000", _M_allocated_capacity = 13827458730995904370}}, joint_state = {header = {seq = 0, stamp = {<ros::TimeBase<ros::Time, ros::Duration>> = {sec = 1550854223, nsec = 634003735}, <No data fields>}, frame_id = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7fff0487fc30 ""}, _M_string_length = 0, {_M_local_buf = '\000' <repeats 15 times>, _M_allocated_capacity = 0}}}, name = {<std::_Vector_base<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >> = {_M_impl = {<std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >> = {<__gnu_cxx::new_allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x246bb60, _M_finish = 0x246bc40, _M_end_of_storage = 0x246bc60}}, <No data fields>}, position = {<std::_Vector_base<double, std::allocator<double> >> = {_M_impl = {<std::allocator<double>> = {<__gnu_cxx::new_allocator<double>> = {<No data fields>}, <No data fields>}, _M_start = 0x246c510, _M_finish = 0x246c548, _M_end_of_storage = 0x246c550}}, <No data fields>}, velocity = {<std::_Vector_base<double, std::allocator<double> >> = {_M_impl = {<std::allocator<double>> = {<__gnu_cxx::new_allocator<double>> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}, effort = {<std::_Vector_base<double, std::allocator<double> >> = {_M_impl = {<std::allocator<double>> = {<__gnu_cxx::new_allocator<double>> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}}, multi_dof_joint_state = {header = {seq = 0, stamp = {<ros::TimeBase<ros::Time, ros::Duration>> = {sec = 0, nsec = 0}, <No data fields>}, frame_id = {static npos = 18446744073709551615, _M_dataplus = {<std::allocator<char>> = {<__gnu_cxx::new_allocator<char>> = {<No data fields>}, <No data fields>}, _M_p = 0x7fff0487fcc0 ""}, _M_string_length = 0, {_M_local_buf = "\000\000\000\000\000\000\000\000\200\376\207\004\377\177\000", _M_allocated_capacity = 0}}}, joint_names = {<std::_Vector_base<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > >> = {_M_impl = {<std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >> = {<__gnu_cxx::new_allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}, transforms = {<std::_Vector_base<geometry_msgs::Transform_<std::allocator<void> >, std::allocator<geometry_msgs::Transform_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<geometry_msgs::Transform_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<geometry_msgs::Transform_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}, twist = {<std::_Vector_base<geometry_msgs::Twist_<std::allocator<void> >, std::allocator<geometry_msgs::Twist_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<geometry_msgs::Twist_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<geometry_msgs::Twist_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}, wrench = {<std::_Vector_base<geometry_msgs::Wrench_<std::allocator<void> >, std::allocator<geometry_msgs::Wrench_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<geometry_msgs::Wrench_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<geometry_msgs::Wrench_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}}, attachable_objects = {<std::_Vector_base<tesseract_msgs::AttachableObject_<std::allocator<void> >, std::allocator<tesseract_msgs::AttachableObject_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<tesseract_msgs::AttachableObject_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<tesseract_msgs::AttachableObject_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x246cb80, _M_finish = 0x246ce98, _M_end_of_storage = 0x246ce98}}, <No data fields>}, attached_bodies = {<std::_Vector_base<tesseract_msgs::AttachedBodyInfo_<std::allocator<void> >, std::allocator<tesseract_msgs::AttachedBodyInfo_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<tesseract_msgs::AttachedBodyInfo_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<tesseract_msgs::AttachedBodyInfo_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x246dda0, _M_finish = 0x246de38, _M_end_of_storage = 0x246de38}}, <No data fields>}, object_colors = {<std::_Vector_base<tesseract_msgs::ObjectColor_<std::allocator<void> >, std::allocator<tesseract_msgs::ObjectColor_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<tesseract_msgs::ObjectColor_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<tesseract_msgs::ObjectColor_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}, highlight_links = {<std::_Vector_base<tesseract_msgs::ObjectColor_<std::allocator<void> >, std::allocator<tesseract_msgs::ObjectColor_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<tesseract_msgs::ObjectColor_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<tesseract_msgs::ObjectColor_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x0, _M_finish = 0x0, _M_end_of_storage = 0x0}}, <No data fields>}, is_diff = 0 '\000', allowed_collisions = {<std::_Vector_base<tesseract_msgs::AllowedCollisionEntry_<std::allocator<void> >, std::allocator<tesseract_msgs::AllowedCollisionEntry_<std::allocator<void> > > >> = {_M_impl = {<std::allocator<tesseract_msgs::AllowedCollisionEntry_<std::allocator<void> > >> = {<__gnu_cxx::new_allocator<tesseract_msgs::AllowedCollisionEntry_<std::allocator<void> > >> = {<No data fields>}, <No data fields>}, _M_start = 0x2471590, _M_finish = 0x2472370, _M_end_of_storage = 0x2472d90}}, <No data fields>}}}
#6  0x00007f558ee3c1bb in trajopt::PlotCosts (plotter=..., joint_names=..., costs=..., cnts=..., vars=..., results=...) at /home/mpowelson/workspaces/temp/src/trajopt/trajopt/src/plot_callback.cpp:41
No locals.

[ahedfares]

@mpowelson , is this bug fixed, i am trying to optimize a problem whilst having the time_stamp for each waypoint as i am executing it later in moveit. I would appreciate if you can advise regardinging any update on this issue.

Thanks