<commit_before>
#include "renderer_robot_plan.hpp"
#include "RobotPlanListener.hpp"
#include "plan_execution_gui_utils.hpp" 
#include "plan_approval_gui_utils.hpp"
#include "plan_store_gui_utils.hpp"

#define PARAM_KP_DEFAULT 5
#define PARAM_KD_DEFAULT 1
#define PARAM_KP_MIN 0
#define PARAM_KD_MIN 0
#define PARAM_KP_MAX 100
#define PARAM_KD_MAX 100
#define PARAM_KP_INC 1
#define PARAM_KD_INC 1

#define RENDERER_NAME "Planning"
#define PARAM_SELECTION "Enable Selection"
#define PARAM_WIRE "Show BBoxs For Meshes"  
#define PARAM_HIDE "Hide Plan"  
//#define PARAM_USE_COLORMAP "Use Colormap"
#define PARAM_PLAN_PART "Part of Plan"  
#define PARAM_SHOW_DURING_CONTROL "During Control"  
#define DRAW_PERSIST_SEC 4
#define PARAM_START_PLAN "Start Planning"
#define PARAM_SEND_COMMITTED_PLAN "Send Plan"
#define PARAM_ADJUST_ENDSTATE "Adjust end keyframe"
#define PARAM_SHOW_FULLPLAN "Show Full Plan"	
#define PARAM_SHOW_KEYFRAMES "Show Keyframes"
#define PARAM_SSE_KP_LEFT "Kp_L"  
#define PARAM_SSE_KD_LEFT "Kd_L"  
#define PARAM_SSE_KP_RIGHT "Kp_R"  
#define PARAM_SSE_KD_RIGHT "Kd_R"
#define PARAM_MANIP_PLAN_MODE "ManipPlnr Mode"
#define PARAM_EXEC_SPEED "EE Speed Limit(cm/s)"
#define PARAM_EXEC_ANG_SPEED "Joint Speed Limit(deg/s)"
#define PARAM_UPDATE_PLANNER_PARAMS "Update Params"
#define PARAM_PLAN_ADJUST_MODE "Plan Adjustment Filter"
#define PARAM_MANIP_PLAN_INITSEED_MODE "ManipPlanFromCurrentState"
#define PARAM_PLAN_USING_BDI_HEIGHT_MODE "Plan & Control w BDI Height"
#define PARAM_ADJUST_PLAN_TO_CURRENT_POSE "Adjust Plan To Current Pose"
#define PARAM_ADJUST_PLAN_AND_REACH "Achieve First Posture"
#define PARAM_COMPENSATE_LAST_FRAME_FOR_SSE "Compensate for SSE"

using namespace std;
using namespace boost;
using namespace renderer_robot_plan;
using namespace renderer_robot_plan_gui_utils;

static void
_renderer_free (BotRenderer *super)
{
  RendererRobotPlan *self = (RendererRobotPlan*) super->user;
  free(self);
}


//=================================


// Convert number to jet colour coordinates
// In: number between 0-->1
// Out: rgb jet colours 0->1
// http://metastine.com/2011/01/implementing-a-continuous-jet-colormap-function-in-glsl/
static inline void jet_rgb(float value,float rgb[]){
  float fourValue = (float) 4 * value;
  rgb[0]   = std::min(fourValue - 1.5, -fourValue + 4.5);
  rgb[1] = std::min(fourValue - 0.5, -fourValue + 3.5);
  rgb[2]  = std::min(fourValue + 0.5, -fourValue + 2.5);
  for (int i=0;i<3;i++){
   if (rgb[i] <0) {
     rgb[i] =0;
   }else if (rgb[i] >1){
     rgb[i] =1;
   }
  }
}


static void 
draw_state(BotViewer *viewer, BotRenderer *super, uint i, float rgb[]){

  float c[3] = {0.3,0.3,0.6}; // light blue
  float alpha = 0.4;
  RendererRobotPlan *self = (RendererRobotPlan*) super->user;
 
  if((self->use_colormap)&&(self->displayed_plan_index==-1)&&(!self->robotPlanListener->_is_manip_plan)) {
    // Each model Jet: blue to red
    float j = (float)i/ (self->robotPlanListener->_gl_robot_list.size() -1);
    jet_rgb(j,c);
  }else{
    c[0] = rgb[0]; c[1] = rgb[1]; c[2] = rgb[2];
  }
  
  glColor4f(c[0],c[1],c[2], alpha);
  
  
  self->robotPlanListener->_gl_robot_list[i]->show_bbox(self->visualize_bbox);
  self->robotPlanListener->_gl_robot_list[i]->enable_link_selection(self->selection_enabled);
  //if((*self->selection)!=" ")
  self->robotPlanListener->_gl_robot_list[i]->highlight_link((*self->selection));
  self->robotPlanListener->_gl_robot_list[i]->draw_body (c,alpha);  
}


static void 
draw_keyframe(BotViewer *viewer, BotRenderer *super, uint i){

  float c_blue[3] = {0.3,0.3,0.6}; // light blue
  float c_green[3] = {0.3,0.6,0.3}; // green
  float c_red[3] = {1.0,0.1,0.1}; // red
  float alpha = 0.7;
  RendererRobotPlan *self = (RendererRobotPlan*) super->user;
  float c[3];
  if((self->use_colormap)) {
    // Each model Jet: blue to red
    float j = (float)i/ (self->robotPlanListener->_gl_robot_keyframe_list.size() -1);
    jet_rgb(j,c);
  }
  else{
   c[0] = c_blue[0];c[1] = c_blue[1];c[2] = c_blue[2];
  }
  
  
  bool markeractive = (self->robotPlanListener->_gl_left_hand->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_right_hand->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_left_foot->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_right_foot->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_pelvis->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_com->is_bodypose_adjustment_enabled());
  
  if(markeractive)
    alpha = 0.2;
  
  glColor4f(c[0],c[1],c[2], alpha);
  
  //self->robotPlanListener->_gl_robot_keyframe_list[i]->enable_link_selection(self->selection_enabled);
  
	std::string selected_keyframe_name = " ";
	//if(self->selected_keyframe_index!=-1)
	
	
	if((markeractive)&&(self->selected_keyframe_index==i))
	{
    self->robotPlanListener->_gl_robot_keyframe_list[i]->highlight_body(selected_keyframe_name);
    self->robotPlanListener->_gl_robot_keyframe_list[i]->draw_body(c,alpha);
  } 
  else if(!markeractive)
  {
  	if(((*self->selection)  != " ")&&(self->selected_keyframe_index!=-1))
	   selected_keyframe_name = self->robotPlanListener->_gl_robot_keyframe_list[self->selected_keyframe_index]->_unique_name; 
    self->robotPlanListener->_gl_robot_keyframe_list[i]->highlight_body(selected_keyframe_name);
    self->robotPlanListener->_gl_robot_keyframe_list[i]->draw_body(c,alpha);
  } 
  
     <commit_msg>critical bug fix for crashes noticed by robin today, only happens with teleop plans are sent outside the stickyhand interface. The issue happened when no of keyframes changed and the keyframe selection was not properly cleared.<commit_after>
#include "renderer_robot_plan.hpp"
#include "RobotPlanListener.hpp"
#include "plan_execution_gui_utils.hpp" 
#include "plan_approval_gui_utils.hpp"
#include "plan_store_gui_utils.hpp"

#define PARAM_KP_DEFAULT 5
#define PARAM_KD_DEFAULT 1
#define PARAM_KP_MIN 0
#define PARAM_KD_MIN 0
#define PARAM_KP_MAX 100
#define PARAM_KD_MAX 100
#define PARAM_KP_INC 1
#define PARAM_KD_INC 1

#define RENDERER_NAME "Planning"
#define PARAM_SELECTION "Enable Selection"
#define PARAM_WIRE "Show BBoxs For Meshes"  
#define PARAM_HIDE "Hide Plan"  
//#define PARAM_USE_COLORMAP "Use Colormap"
#define PARAM_PLAN_PART "Part of Plan"  
#define PARAM_SHOW_DURING_CONTROL "During Control"  
#define DRAW_PERSIST_SEC 4
#define PARAM_START_PLAN "Start Planning"
#define PARAM_SEND_COMMITTED_PLAN "Send Plan"
#define PARAM_ADJUST_ENDSTATE "Adjust end keyframe"
#define PARAM_SHOW_FULLPLAN "Show Full Plan"	
#define PARAM_SHOW_KEYFRAMES "Show Keyframes"
#define PARAM_SSE_KP_LEFT "Kp_L"  
#define PARAM_SSE_KD_LEFT "Kd_L"  
#define PARAM_SSE_KP_RIGHT "Kp_R"  
#define PARAM_SSE_KD_RIGHT "Kd_R"
#define PARAM_MANIP_PLAN_MODE "ManipPlnr Mode"
#define PARAM_EXEC_SPEED "EE Speed Limit(cm/s)"
#define PARAM_EXEC_ANG_SPEED "Joint Speed Limit(deg/s)"
#define PARAM_UPDATE_PLANNER_PARAMS "Update Params"
#define PARAM_PLAN_ADJUST_MODE "Plan Adjustment Filter"
#define PARAM_MANIP_PLAN_INITSEED_MODE "ManipPlanFromCurrentState"
#define PARAM_PLAN_USING_BDI_HEIGHT_MODE "Plan & Control w BDI Height"
#define PARAM_ADJUST_PLAN_TO_CURRENT_POSE "Adjust Plan To Current Pose"
#define PARAM_ADJUST_PLAN_AND_REACH "Achieve First Posture"
#define PARAM_COMPENSATE_LAST_FRAME_FOR_SSE "Compensate for SSE"

using namespace std;
using namespace boost;
using namespace renderer_robot_plan;
using namespace renderer_robot_plan_gui_utils;

static void
_renderer_free (BotRenderer *super)
{
  RendererRobotPlan *self = (RendererRobotPlan*) super->user;
  free(self);
}


//=================================


// Convert number to jet colour coordinates
// In: number between 0-->1
// Out: rgb jet colours 0->1
// http://metastine.com/2011/01/implementing-a-continuous-jet-colormap-function-in-glsl/
static inline void jet_rgb(float value,float rgb[]){
  float fourValue = (float) 4 * value;
  rgb[0]   = std::min(fourValue - 1.5, -fourValue + 4.5);
  rgb[1] = std::min(fourValue - 0.5, -fourValue + 3.5);
  rgb[2]  = std::min(fourValue + 0.5, -fourValue + 2.5);
  for (int i=0;i<3;i++){
   if (rgb[i] <0) {
     rgb[i] =0;
   }else if (rgb[i] >1){
     rgb[i] =1;
   }
  }
}


static void 
draw_state(BotViewer *viewer, BotRenderer *super, uint i, float rgb[]){

  float c[3] = {0.3,0.3,0.6}; // light blue
  float alpha = 0.4;
  RendererRobotPlan *self = (RendererRobotPlan*) super->user;
 
  if((self->use_colormap)&&(self->displayed_plan_index==-1)&&(!self->robotPlanListener->_is_manip_plan)) {
    // Each model Jet: blue to red
    float j = (float)i/ (self->robotPlanListener->_gl_robot_list.size() -1);
    jet_rgb(j,c);
  }else{
    c[0] = rgb[0]; c[1] = rgb[1]; c[2] = rgb[2];
  }
  
  glColor4f(c[0],c[1],c[2], alpha);
  
  
  self->robotPlanListener->_gl_robot_list[i]->show_bbox(self->visualize_bbox);
  self->robotPlanListener->_gl_robot_list[i]->enable_link_selection(self->selection_enabled);
  //if((*self->selection)!=" ")
  self->robotPlanListener->_gl_robot_list[i]->highlight_link((*self->selection));
  self->robotPlanListener->_gl_robot_list[i]->draw_body (c,alpha);  
}


static void 
draw_keyframe(BotViewer *viewer, BotRenderer *super, uint i){
   
  float c_blue[3] = {0.3,0.3,0.6}; // light blue
  float c_green[3] = {0.3,0.6,0.3}; // green
  float c_red[3] = {1.0,0.1,0.1}; // red
  float alpha = 0.7;
  RendererRobotPlan *self = (RendererRobotPlan*) super->user;
  float c[3];
  if((self->use_colormap)) {
    // Each model Jet: blue to red
    float j = (float)i/ (self->robotPlanListener->_gl_robot_keyframe_list.size() -1);
    jet_rgb(j,c);
  }
  else{
   c[0] = c_blue[0];c[1] = c_blue[1];c[2] = c_blue[2];
  }
  
  
  bool markeractive = (self->robotPlanListener->_gl_left_hand->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_right_hand->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_left_foot->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_right_foot->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_pelvis->is_bodypose_adjustment_enabled());
  markeractive = markeractive||(self->robotPlanListener->_gl_com->is_bodypose_adjustment_enabled());
  
  if(markeractive)
    alpha = 0.2;
  
  glColor4f(c[0],c[1],c[2], alpha);
  
  //self->robotPlanListener->_gl_robot_keyframe_list[i]->enable_link_selection(self->selection_enabled);
  
	std::string selected_keyframe_name = " ";
	//if(self->selected_keyframe_index!=-1)
	
	
	if((markeractive)&&(self->selected_keyframe_index==i))
	{
    self->robotPlanListener->_gl_robot_keyframe_list[i]->highlight_body(selected_keyframe_name);
    self->robotPlanListener->_gl_robot_keyframe_list[i]->draw_body(c,alpha);
  } 
  else if(!markeractive)
  {
  	if(((*self->selection)  != " ")
  	    &&(self->selected_keyframe_index!=-1)
  	    &&(self->selected_keyframe_index < self->robotPlanListener->_gl_robot_keyframe_list.size()))
	   selected_keyframe_name = self->robotPlanListener->_gl_robot_keyframe_list[self->selected_keyframe_index]->_unique_name; 
    self->robotPlanListener->_gl_robot_keyframe_list[i]->highlight_body(selected_keyframe_name);
    self->robotPlanListener->_gl_robot_keyframe_list[i]->draw_body(c,alpha);
  } 
  
     
{'fraction_non_alphanumeric': 0.1423478260869565, 'fraction_numerical': 0.018260869565217393, 'mean_word_length': 5.6059735784032165, 'pattern_counts': {'":': 0, '<': 8, '<?xml version=': 0, '>': 152, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7589129', 'n_tokens_mistral': 4543, 'n_tokens_neox': 4360, 'n_words': 759}